def solveIsolatedImpulseLambda(self,
velocityJacobian,
angJacobian,
bias=0.0):
em = Vector.dot(velocityJacobian, velocityJacobian) / self.mass
em += self.rotationVectorDotProduct(
angJacobian, (self.inverseInertia * angJacobian))
em = 1.0 / em
deviation = Vector.dot(velocityJacobian, self.velocity)
rot = self.angMomToRotMat(self.R, self.inverseInertia) * self.L
deviation += self.rotationVectorDotProduct(angJacobian, rot)
return -em * (deviation + bias)
def globalFrontDrivingDir(self):
tc = self.turningCenter()
if tc == None: return self.globalRearDrivingDir()
r = self.localToGlobal(self.axisPos(FRONT)) - tc
d = Vector([-r.y, r.x])
if Vector.dot(d, self.globalRearDrivingDir()) < 0: d = -d
return d
def test_rigid_body_rotation_and_torque(self): torque = Vector([0,0,2.0]) dt = 0.25 rigidBody = self.newRigidBody() rigidBody.applyTorque( torque ) self.assertVecsEqual( rigidBody.torque, torque ) rigidBody.integrateAppliedForces(dt) self.assertVecsEqual( rigidBody.torque, Vector([0,0,0]) ) self.assertVecsEqual( rigidBody.velocity, Vector([0,0,0]) ) self.assertVecsEqual( rigidBody.position, Vector([0,0,0]) ) self.assertVecsEqual( rigidBody.force, Vector([0,0,0]) ) expectedL = torque * dt self.assertVecsEqual( rigidBody.L, expectedL ) self.assertEqual( rigidBody.R._e, Matrix.identity(3)._e ) rigidBody.integratePosition(dt) self.assertVecsEqual( rigidBody.L, expectedL ) self.assertVecsEqual( Vector([0,0,1]), rigidBody.R.column(2) ) self.assertVecsEqual( Vector([0,0,1]), rigidBody.R.transpose().column(2) ) self.assertEqual( 0, Vector.dot( rigidBody.R.column(1), rigidBody.R.column(2) ) )
def orthogonalize(m):
"""
Orthogonalize a matrix by applying Gram-Shmidt to
its columns in ascending order
"""
c1 = m.column(0)
c2 = m.column(1)
c3 = m.column(2)
c1.normalize()
c2 += c1 * (-Vector.dot(c2, c1))
c2.normalize()
c3 += c1 * (-Vector.dot(c3, c1))
c3 += c2 * (-Vector.dot(c3, c2))
c3.normalize()
m._e = c1._e + c2._e + c3._e
def test_dot(self): v1, orig_v1 = self.new_vector_and_copy() v2, orig_v2 = self.new_vector_and_copy() result = Vector.dot( v1, v2 ) expected = sum([ orig_v1[i] * orig_v2[i] for i in range(self.vec_dim) ]) self.assertEqual( result, expected ) self.assertEqual( v1._e, orig_v2._e ) self.assertEqual( v2._e, orig_v2._e )
def render(self):
glPushMatrix()
glTranslate(self.position.x, self.position.y, 0.0)
totalForces = self.totalForces[REAR] + self.totalForces[FRONT]
sideForces = Vector.dot(totalForces, self.globalAxisDir())
frontForces = Vector.dot(totalForces, self.globalRearDrivingDir())
sideTilt = sideForces / self.mass * 1.1
frontTilt = -frontForces / self.mass * 0.6
glRotate(self.R / math.pi * 180, 0, 0, 1)
glRotate(sideTilt, 1, 0, 0)
glRotate(frontTilt, 0, 1, 0)
glTranslate(0, 0, 0.8)
glColor(0.8, 0.5, 0.5)
if USE_GLUT:
glPushMatrix()
depth = 0.5
glScale(self.width, self.height, depth)
glutSolidCube(1.0)
glPopMatrix()
else:
glBegin(GL_QUADS)
#glNormal(0,0,-1)
for v in self.localVertexPositions():
glVertex(v.x, v.y, 0)
glEnd()
def drawLight(value, offset, width, height, color):
glColor(*(color + (value, )))
if not USE_GLUT: return drawBar(value, offset, width)
glEnable(GL_BLEND)
glDisable(GL_LIGHTING)
for side in [-1, 1]:
glPushMatrix()
glTranslate(-self.width * 0.5,
side * (self.height * offset * 0.5), 0)
glScale(0.1, width, depth * height)
glutSolidCube(1.0)
glPopMatrix()
glEnable(GL_LIGHTING)
glDisable(GL_BLEND)
def drawBar(value, offset, width, elevation=0.01):
if USE_GLUT:
elevation += depth * 0.5
hh = self.height * 0.5 * value
hw = self.width * 0.5 * width
w0 = self.width * (offset - 0.5)
vert = [
Vector([-hw + w0, -hh, elevation]),
Vector([hw + w0, -hh, elevation]),
Vector([hw + w0, hh, elevation]),
Vector([-hw + w0, hh, elevation])
]
glNormal(0, 0, 1)
glBegin(GL_QUADS)
for v in vert:
glVertex(*v._e)
glEnd()
# Draw throttle and brake bars
drawLight(self.curBrake, 0.7, 0.3, 0.5, (1.0, 0.0, 0.0))
drawLight(self.curThrottle, 0.25, 0.4, 0.1, (1.0, 0.7, 0.0))
# Traction limit indicators
glColor(0.3, 0.3, 0.3)
frontTraction = self.wheelConstraints[FRONT].lambdaRatio()
rearTraction = self.wheelConstraints[REAR].lambdaRatio()
glColor(0.5, 0.5, 0.5)
if frontTraction != None:
drawBar(frontTraction, 0.9, 0.1, 0.02)
if rearTraction != None:
drawBar(rearTraction, 0.1, 0.1, 0.02)
glPopMatrix()
self.drawWheel(self.wheelPos(REAR, -1), False)
self.drawWheel(self.wheelPos(REAR, 1), False)
self.drawWheel(self.wheelPos(FRONT, -1), True)
self.drawWheel(self.wheelPos(FRONT, 1), True)
def move(self, dt, game):
saDelta = (self.targetSteeringAngle -
self.steeringAngle) * dt * STEERING_SENSITIVITY
self.steeringAngle += saDelta
def clampedControl(what, control, up, down):
if control: return min(what + up * dt, 1.0)
else: return max(what - down * dt, 0.0)
self.curThrottle = clampedControl(self.curThrottle, self.throttle,
THROTTLE_UP, THROTTLE_DOWN)
self.curBrake = clampedControl(self.curBrake, self.brake, BRAKE_UP,
BRAKE_DOWN)
if self.curThrottle > 0: self.curBrake = 0.0
self.axisWeight = {
REAR: self.mass * self.rearWeightRatio * GRAVITY,
FRONT: self.mass * (1.0 - self.rearWeightRatio) * GRAVITY
}
# compute weight transfer based on last time
longForces = Vector.dot(
self.totalForces[FRONT] + self.totalForces[REAR],
self.globalRearDrivingDir())
WT_RATIO = 0.05 / 2.0 # center of mass height / wheelbase
weightTransfer = longForces * WT_RATIO * GRAVITY
self.axisWeight[FRONT] -= weightTransfer
self.axisWeight[REAR] += weightTransfer
friction = {}
maxAxisFriction = {}
axisPos = {}
axisVel = {}
axisTravelDir = {}
axisForces = {}
for ax in AXES:
if self.sliding[ax]: friction[ax] = DYNAMIC_FRICTION
else: friction[ax] = STATIC_FRICTION
maxAxisFriction[ax] = self.axisWeight[ax] * friction[ax]
axisPos[ax] = self.localToGlobal(self.axisPos(ax))
axisVel[ax] = self.globalPointVelocity(axisPos[ax])
axisTravelDir[ax] = axisVel[ax].safeDirection()
axisForces[ax] = Vector([0, 0])
# rolling resistance
if self.velocity.norm() > 1.0:
axisForces[ax] += -axisTravelDir[ax] * self.axisWeight[
FRONT] * ROLLING_RESISTANCE
BRAKE_COEFFICIENT = 0.9
frictionLeft = lambda total, used: math.sqrt(1.0 - min(
(used / total)**2, 1.0)) * total
if self.curBrake > 0:
coeff = STATIC_FRICTION * BRAKE_COEFFICIENT * self.curBrake
for ax in AXES:
braking = min(self.axisWeight[ax] * coeff, maxAxisFriction[ax])
braking *= min(1.0, axisVel[ax].norm())
force = -axisTravelDir[ax] * braking
axisForces[ax] += force
maxAxisFriction[ax] = frictionLeft(maxAxisFriction[ax],
braking)
elif self.curThrottle > 0:
POWER = self.mass * 200.0
throttleMagn = min(
self.curThrottle * POWER / (self.velocity.norm() + 0.5),
maxAxisFriction[REAR] * 0.90)
throttleForce = self.globalRearDrivingDir() * throttleMagn
axisForces[REAR] += throttleForce
maxAxisFriction[REAR] = frictionLeft(maxAxisFriction[REAR],
throttleMagn)
axdir = self.globalAxisDir()
for ax in AXES:
self.applyForce(axisForces[ax], axisPos[ax])
self.wheelConstraints[REAR].noMoveDir = axdir
# air resistance
self.applyForceCm(-self.velocity * self.velocity.norm() *
AIR_RESISTANCE)
self.integrateAppliedForces(dt)
if self.currentTurningRadius != None:
c = self.turningCenter()
p = self.localToGlobal(self.axisPos(FRONT))
self.wheelConstraints[FRONT].noMoveDir = (p - c).direction()
for ax in AXES:
self.wheelConstraints[ax].setAbsLambda(maxAxisFriction[ax] * dt)
self.constraints = self.wheelConstraints
vertices = self.vertexPositions()
for v in vertices:
n = game.crossedNormal(v)
if n != None:
c = rigidbody.SingleBodyPointVelocityConstraint(self, v, n)
c.minLambda = 0.0
self.constraints.append(c)
for c in self.constraints:
c.resetAndComputeJacobians()
N_ITER = 10
for i in range(N_ITER):
for c in self.constraints:
c.applyImpulses()
for ax in AXES:
self.totalForces[
ax] = axisForces[ax] + self.wheelConstraints[ax].totalForce(dt)
for side in (-1, 1):
wheel = self.localToGlobal(self.wheelPos(ax, side))
self.integratePosition(dt)
for ax in (FRONT, REAR):
self.sliding[ax] = self.wheelConstraints[ax].lambdaActive()
def rotationVectorDotProduct(self, v1, v2):
return Vector.dot(v1, v2)