def uniformlyAcceleratingRotationTest(self, integrationMethod):
constantZMoment = ForceMomentSystem(self.zeroVec,
moment=Vector(0, 0, 1))
restingState = RigidBodyState(self.zeroVec, self.zeroVec,
self.zeroQuat, self.zeroAngVel)
def constZMomentFunc(*allArgs):
return constantZMoment
def constInertiaFunc(*allArgs):
return self.constInertia
acceleratingZRotBody = RigidBody(restingState,
constZMomentFunc,
constInertiaFunc,
integrationMethod=integrationMethod,
simDefinition=self.simDefinition)
acceleratingZRotBody.timeStep(1)
newOrientation = acceleratingZRotBody.state.orientation
newAngVel = acceleratingZRotBody.state.angularVelocity
#print("{}: {}".format(integrationMethod, newOrientation))
assertAngVelAlmostEqual(
self, newAngVel,
AngularVelocity(axisOfRotation=Vector(0, 0, 1), angularVel=1))
if integrationMethod == "Euler":
assertQuaternionsAlmostEqual(
self, newOrientation,
Quaternion(axisOfRotation=Vector(0, 0, 1), angle=0))
else:
assertQuaternionsAlmostEqual(
self, newOrientation,
Quaternion(axisOfRotation=Vector(0, 0, 1), angle=0.5))
def test_MultiplyRigidBodyState(self):
iRBS3 = self.iRBS2 * 0.5
self.assertEqual(iRBS3.position, Vector(0.5, 0, 0))
self.assertEqual(iRBS3.velocity, Vector(1, 0, 0))
assertQuaternionsAlmostEqual(self,
iRBS3.orientation,
Quaternion(axisOfRotation=Vector(1, 0, 0),
angle=pi / 4),
n=14)
self.nearlyEqualAngVel(
iRBS3.angularVelocity,
AngularVelocity(axisOfRotation=Vector(1, 0, 0), angularVel=pi / 2))
def test_interpolateRigidBodyStates(self):
vel1 = Vector(0, 1, 2)
vel2 = Vector(2, 3, 4)
pos1 = vel1
pos2 = Vector(4, 5, 6)
angVel1 = vel1
angVel2 = Vector(6, 7, 8)
quat1 = Quaternion(axisOfRotation=Vector(0, 0, 1), angle=0)
quat2 = Quaternion(axisOfRotation=Vector(0, 0, 1), angle=2)
rBS1 = RigidBodyState(pos1, vel1, quat1, angVel1)
rBS2 = RigidBodyState(pos2, vel2, quat2, angVel2)
rBS1p5 = interpolateRigidBodyStates(rBS1, rBS2, 0.5)
assertIterablesAlmostEqual(self, Vector(2, 3, 4), rBS1p5.position)
assertIterablesAlmostEqual(self, Vector(1, 2, 3), rBS1p5.velocity)
assertQuaternionsAlmostEqual(
self, rBS1p5.orientation,
Quaternion(axisOfRotation=Vector(0, 0, 1), angle=1))
assertIterablesAlmostEqual(self, rBS1p5.angularVelocity,
Vector(3, 4, 5))
def uniformZRotationTest(self, integrationMethod):
rotatingZState = RigidBodyState(
self.zeroVec, self.zeroVec, self.zeroQuat,
AngularVelocity(axisOfRotation=Vector(0, 0, 1), angularVel=pi))
constInertia = Inertia(self.oneVec, self.zeroVec, 1)
def constInertiaFunc(*allArgs):
return constInertia
def constForceFunc(*allArgs):
return self.zeroForce
rotatingZBody = RigidBody(rotatingZState,
constForceFunc,
constInertiaFunc,
integrationMethod=integrationMethod,
simDefinition=self.simDefinition)
rotatingZBody.timeStep(1)
newOrientation = rotatingZBody.state.orientation
expectedOrientation = Quaternion(axisOfRotation=Vector(0, 0, 1),
angle=pi)
assertQuaternionsAlmostEqual(self, newOrientation, expectedOrientation)
def test_slerp(self):
testQ = self.q1.slerp(self.q3, 0)
assertQuaternionsAlmostEqual(self, (self.q1 * testQ).normalize(),
self.q1.normalize())
testQ = self.q1.slerp(self.q3, 1)
testQ = (self.q1 * testQ).normalize()
assertQuaternionsAlmostEqual(self, testQ, self.q3.normalize())
testQ = self.rotQ2.slerp(self.rotQ5, 0.5)
testQ = (self.rotQ2 * testQ).normalize()
assertQuaternionsAlmostEqual(
self, testQ,
Quaternion(axisOfRotation=Vector(0, 0, 1), angle=3 * pi / 4))
def test_constructor(self):
assertQuaternionsAlmostEqual(
self, self.q1, Quaternion(components=[1.0, 2.0, 3.0, 4.0]))
expectedQ3 = Quaternion(components=[sin(pi / 4), sin(pi / 4), 0, 0])
assertQuaternionsAlmostEqual(self, self.q3, expectedQ3)
def test_scaleRotation(self):
scaledQuat = self.q3.scaleRotation(0.5)
definedQuat = Quaternion(axisOfRotation=Vector(1, 0, 0), angle=pi / 4)
assertQuaternionsAlmostEqual(self, scaledQuat, definedQuat, 14)
def test_inverse(self):
assertQuaternionsAlmostEqual(
self, self.q4.inverse(),
Quaternion(components=[0.5, 0.0, -0.5, 0.0]))
def test_conjugate(self):
assertQuaternionsAlmostEqual(self, self.q4.conjugate(),
Quaternion(components=[1, 0, -1, 0]))
def test_normalize(self):
testQ = self.q4.normalize()
expectedQ = Quaternion(components=[0.7071, 0.0, 0.7071, 0.0])
assertQuaternionsAlmostEqual(self, testQ, expectedQ, 3)
def test_quaternionDivision(self):
testQ = self.q4.__truediv__(self.q5)
expectedQ = Quaternion(components=[0.7273, 0.1212, 0.2424, -0.6061])
assertQuaternionsAlmostEqual(self, testQ, expectedQ, 3)
