def drawWheel(self, pos, frontWheel):
pos = self.localToGlobal(pos)
if frontWheel and self.currentTurningRadius != None:
tc = self.turningCenter()
ax = (pos - tc).direction()
direction = self.rotationVectorCrossProduct(1, ax).direction()
else:
direction = self.globalRearDrivingDir()
ax = self.globalAxisDir()
wheelR = self.width * 0.15
col = (0.3, 0.3, 0.3)
if USE_GLUT:
glPushMatrix()
glColor(*col)
glTranslate(pos.x, pos.y, wheelR)
glRotate(
math.atan2(direction.y, direction.x) / math.pi * 180.0, 0, 0,
1)
glRotate(90, 1, 0, 0)
cylH = 0.3
glTranslate(0, 0, -cylH * 0.5)
glutSolidCylinder(wheelR, cylH, 16, 1)
glPopMatrix()
else:
drawCircle3d(Vector([pos.x, pos.y, wheelR]),
Vector([ax.x, ax.y, 0]), wheelR, col)
def crossedNormal(self, point):
if point.x < 0: return Vector([1, 0])
if point.y < 0: return Vector([0, 1])
if point.x > self.width: return Vector([-1, 0])
if point.y > self.height: return Vector([0, -1])
return None
def test_rigid_body_initial(self): rigidBody = self.newRigidBody() self.assertVecsEqual( rigidBody.velocity, Vector([0,0,0]) ) self.assertVecsEqual( rigidBody.position, Vector([0,0,0]) ) self.assertVecsEqual( rigidBody.torque, Vector([0,0,0]) ) self.assertVecsEqual( rigidBody.L, Vector([0,0,0]) )
def localVertexPositions(self):
hw = self.width * 0.5
hh = self.height * 0.5
return [
Vector([-hw, -hh]),
Vector([hw, -hh]),
Vector([hw, hh]),
Vector([-hw, hh])
]
def test_rigid_body_apply_force(self): at = Vector([0, 2.0]) force = Vector([-1.0, 0]) dt = 0.5 rigidBody = self.newRigidBody() rigidBody.applyForce( force, at ) self.assertEqual( rigidBody.torque, 2.0 ) self.assertVecsEqual( rigidBody.force, force )
def test_mul_vec(self): m1 = [[1,2,3], [4,5,6]] M1 = Matrix.fromRowMajor( m1 ) vec = Vector([1,0,1]) result = M1 * vec self.assertEqual( Vector([4,10])._e, result._e ) self.assertEqual( m1, M1.rowMajorArrays() )
def test_cross_product( self ): space = rigidbody.Space2D() v1 = Vector([1,0]) v2 = Vector([0,2]) self.assertEqual( space.vectorVectorCrossProduct( v1, v2 ), 2 ) self.assertEqual( space.vectorVectorCrossProduct( v2, v1 ), -2 ) self.assertEqual( space.vectorVectorCrossProduct( v1, v1 ), 0 ) self.assertEqual( space.rotationVectorCrossProduct( -2, v1 )._e, Vector([0,-2])._e ) self.assertEqual( space.rotationVectorDotProduct( 2, 3 ), 6 )
def __init__(self):
self.screenScale = 1.0 / TRACK_SIZE
self.width = 1.0 / self.screenScale
self.height = SCREENTOP / self.screenScale
self.track = Track(Vector([self.width / 2, self.height / 2]),
self.width * 0.2)
self.car = Car()
self.car.position = Vector(self.track.points[0])
self.T = 0
self.cameraheading = 0.0
def __init__(self):
width = 2.5
height = 2
mass = 500.0
inertia = 1 / 12.0 * mass * (width**2 + height**2)
rigidbody.RigidBody.__init__(self, mass, 1.0 / inertia)
self.width = width
self.height = height
self.rearWeightRatio = 0.52
self.maxSteeringAngle = math.pi / 2
self.steeringAngle = 0.0
self.targetSteeringAngle = 0.0
self.curThrottle = 0.0
self.curBrake = 0.0
self.totalForces = [Vector([0, 0]) for ax in AXES]
self.sliding = {REAR: False, FRONT: False}
self.slideLength = 0
Constraint = rigidbody.SingleBodyRelativePointVelocityConstraint
self.wheelConstraints = [
Constraint(self, self.axisPos(ax), self.globalAxisDir())
for ax in AXES
]
def test_new_vector(self): v = Vector([3,4]) self.assertEqual(v.dim, 2) self.assertEqual(v.x, 3) self.assertEqual(v.y, 4) self.assertEqual(v.norm(), 5.0) self.assertEqual([3,4], v._e)
def test_rigid_body_apply_force(self): at = Vector([0, 2.0, 0]) force = Vector([-1.0, 0, 0]) dt = 0.5 rigidBody = self.newRigidBody() rigidBody.applyForce( force, at ) self.assertVecsEqual( rigidBody.torque, Vector([0,0,2]) ) self.assertVecsEqual( rigidBody.force, force ) rigidBody.move(dt) self.assertVecsEqual( Vector([0,0,1]), rigidBody.R.column(2) ) self.assertVecsEqual( Vector([0,0,1]), rigidBody.R.transpose().column(2) )
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 render(self):
CAMERA_DISTANCE = 5
camerapitch = -70
glLoadIdentity()
sc = self.screenScale
glScale(sc, sc, sc)
if PERSPECTIVE:
# Transform to world coordinates by positioning camera
glTranslate(0, -1, -CAMERA_DISTANCE)
glRotate(camerapitch, 1, 0, 0)
glRotate(self.cameraheading, 0, 0, 1)
glTranslate(-self.car.position.x, -self.car.position.y, 0)
glLight(GL_LIGHT0, GL_POSITION, (1, 1, 3, 0))
glEnable(GL_LIGHTING)
glEnable(GL_TEXTURE_2D)
glBegin(GL_QUADS)
glNormal(0, 0, 1)
vertices = [
Vector([0, 0]),
Vector([self.width, 0]),
Vector([self.width, self.height]),
Vector([0, self.height])
]
texScale = 0.1
for v in vertices:
glTexCoord2f(v.x * texScale, v.y * texScale)
glVertex(v.x, v.y, 0.0)
glEnd()
glDisable(GL_TEXTURE_2D)
glEnable(GL_COLOR_MATERIAL)
self.track.render()
self.car.render()
text = " %.01f s, " % self.T
vel = self.car.velocity.norm()
text += "%d km/h" % int(vel * 3.6)
drawText(text)
def test_neg(self): v, orig_v = self.new_vector_and_copy() v = Vector( self.new_elem_list() ) result = -v self.assertEqual( result._e, [ -orig_v[i] for i in range(self.vec_dim) ] ) self.assertEqual( v._e, orig_v._e )
def test_new_vector(self): v = Vector([1,2,3]) self.assertEqual(v.dim, 3) self.assertEqual(v.x, 1) self.assertEqual(v.y, 2) self.assertEqual(v.z, 3) self.assertEqual(v.norm()**2, float(1+4+9)) self.assertEqual([1,2,3], v._e)
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 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()
def drawCircle3d(center3d, axis3d, radius, color, npoints=32):
axis3d.normalize()
d1 = Vector.cross(axis3d, Vector([0, 0, 1])).direction()
d2 = Vector.cross(axis3d, d1)
glColor(*color)
glBegin(GL_TRIANGLE_FAN)
glVertex(*center3d._e)
for i in xrange(npoints + 1):
th = (i / float(npoints)) * 2 * math.pi
p = center3d + (d1 * math.sin(th) + d2 * math.cos(th)) * radius
glVertex(*p._e)
glEnd()
def test_rigid_body_rotation_and_torque(self): torque = 2.0 dt = 0.5 rigidBody = self.newRigidBody() rigidBody.applyTorque( torque ) self.assertEqual( rigidBody.torque, torque ) rigidBody.integrateAppliedForces(dt) self.assertEqual( rigidBody.torque, 0.0 ) self.assertVecsEqual( rigidBody.velocity, Vector([0,0]) ) self.assertVecsEqual( rigidBody.position, Vector([0,0]) ) self.assertVecsEqual( rigidBody.force, Vector([0,0]) ) expectedL = dt * torque self.assertEqual( rigidBody.L, expectedL ) self.assertEqual( rigidBody.R, 0.0 ) rigidBody.integratePosition(dt) self.assertEqual( rigidBody.L, expectedL ) self.assertEqual( rigidBody.R, rigidBody.inverseInertia * expectedL * dt )
def rotationVectorCrossProduct(self, rotation, vec):
return Vector([-vec.y, vec.x]) * rotation
def zeroVector(self):
return Vector([0, 0, 0])
def mouse(self, xy, buttons):
x, y = xy
clickPos = Vector([x, y]) * (1.0 / self.screenScale)
self.car.setControls(x, y, clickPos, buttons)
def new_vector_and_copy(self): els = self.new_elem_list() return ( Vector( els ), Vector( els ) )
def vecTo3D(v):
return Vector([v.x, v.y, 0])
def randDir(self):
return Vector([random.normalvariate(0, 1) for i in [1, 2]]).direction()
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 axisPos(self, ax):
if ax == REAR: return Vector([-1 + self.cmShift, 0])
elif ax == FRONT: return Vector([1 + self.cmShift, 0])
def localAxisDir(self):
return Vector([0, 1])
def globalAxisDir(self):
return self.localToGlobal(Vector([0, 1])) - self.position
def globalRearDrivingDir(self):
return self.localToGlobal(Vector([1, 0])) - self.position