def _getCPZ(componentForces) -> int:
normalForce = Vector(componentForces.force.X, componentForces.force.Y, 0)
normalForceMagnitude = normalForce.length()
if normalForceMagnitude == 0.0:
# Avoid division by zero, just return CG location
return componentForces.location.Z
normalForceDirection = normalForce.normalize()
momentAxis = normalForceDirection.crossProduct(Vector(0, 0, 1))
momentComponent = componentForces.moment * momentAxis
return componentForces.location.Z - momentComponent / normalForceMagnitude
class RigidBodyState():
""" Class created to be able to treat rigidBody states like scalars when integrating the movement of a rigid body
Pos/Vel are expected to be Vectors - Defined with reference to the global frame
Orientation is expected to be a Quaternion - Defines the rotation from the global inertial reference frame to the rocket's local frame
(Orientation of the rocket in the global frame)
Angular Velocity is expected to be an Angular Velocity - Defined with reference to the local frame
Adding rigidBodyStates adds the vectors and multiplies the quaternions (which adds the rotations they represent)
Multiplying an rigidBodyState by a scalar scales the vectors and rotation defined by the quaternions
0.5 * = half the vector length, half the rotation size, directions the same
No other operations are defined
"""
def __init__(self,
position=None,
velocity=None,
orientation=None,
angularVelocity=None):
self.position = Vector(0, 0, 0) if (position is None) else position
self.velocity = Vector(0, 0, 0) if (velocity is None) else velocity
self.orientation = Quaternion(
1, 0, 0, 0) if (orientation is None) else orientation
self.angularVelocity = AngularVelocity(
0, 0, 0) if (angularVelocity is None) else angularVelocity
def __add__(self, rigidBodyState2):
''' Used in: initVal {+} (timeStep * slope) '''
newPos = self.position + rigidBodyState2.position
newVel = self.velocity + rigidBodyState2.velocity
newAngVel = self.angularVelocity + rigidBodyState2.angularVelocity
newOrientation = rigidBodyState2.orientation * self.orientation.normalize(
)
return RigidBodyState(newPos, newVel, newOrientation.normalize(),
newAngVel)
def __mul__(self, timeStep):
'''
Used in: initVal + timeStep {*} slope
Expected to always be multiplied by a scalar
'''
timeStep = float(timeStep)
newPos = self.position * timeStep
newVel = self.velocity * timeStep
newAngVel = self.angularVelocity * timeStep
newOrientation = self.orientation.scaleRotation(timeStep)
return RigidBodyState(newPos, newVel, newOrientation, newAngVel)
def __abs__(self):
''' Used to quantify the difference between two RigidBodyStates as a scalar value during error estimation in adaptive time stepping methods '''
positionMag = self.position.length() + self.velocity.length()
orientationMag = abs(
self.orientation.rotationAngle()) + self.angularVelocity.angVel()
return orientationMag * 100 + positionMag
def __eq__(self, iRBS2):
try:
properties = [
self.position, self.velocity, self.orientation,
self.angularVelocity
]
otherProperties = [
iRBS2.position, iRBS2.velocity, iRBS2.orientation,
iRBS2.angularVelocity
]
return all([x == y for (x, y) in zip(properties, otherProperties)])
except AttributeError:
return False
def __str__(self):
''' Called by print function '''
return " {:>10.3f} {:>10.4f} {:>11.7f} {:>9.4f}".format(
self.position, self.velocity, self.orientation,
self.angularVelocity)
### Wrapper/Thin functions ###
def __neg__(self):
return RigidBodyState(self.position * -1, self.velocity * -1,
self.orientation.conjugate(),
self.angularVelocity * -1)
class TestVector(unittest.TestCase):
def setUp(self):
#Define Vectors to be used in the checks below
self.v1 = Vector(1.0,2.0,3.0)
self.v2 = Vector(-1.0,-2.0,-3.0)
self.v3 = Vector(2.0,2.0,2.0)
self.v4 = Vector(1.0,-3.0,4.0)
self.v5 = Vector(3.0,4.0,0.0)
self.v6 = Vector(4.0,4.0,2.0)
self.v7 = Vector(1.0,0.0,3.0)
self.v8 = Vector(5.0,5.0,0.0)
def test_format(self):
formattedVector = "{:1.2f}".format(self.v1)
self.assertEqual(formattedVector, "1.00 2.00 3.00")
def test_getItem(self):
self.assertEqual(self.v1[0], 1)
self.assertEqual(self.v1[1], 2)
self.assertEqual(self.v1[2], 3)
def test_iter(self):
x, y, z = self.v1
self.assertEqual(x, 1)
self.assertEqual(y, 2)
self.assertEqual(z, 3)
def test_constructorParsing(self):
self.assertEqual(Vector("(1,2,3)"), self.v1)
self.assertEqual(Vector("(1, 2, 3)"), self.v1)
self.assertEqual(Vector("(1;2;3)"), self.v1)
self.assertEqual(Vector("(1; 2; 3)"), self.v1)
self.assertEqual(Vector("(1 2 3)"), self.v1)
self.assertEqual(Vector(" (694.25 200 3) "), Vector(694.25, 200, 3))
self.assertEqual(Vector(" (694.25,200,3) "), Vector(694.25, 200, 3))
self.assertEqual(Vector("(0.0 0.0 0.0)"), Vector(0,0,0))
def test_Multiplication(self):
self.assertEqual(self.v1*0.0,Vector(0,0,0))
self.assertEqual(self.v1*1.0, self.v1)
self.assertEqual(self.v1*(-1), self.v2)
def test_rMul(self):
assertVectorsAlmostEqual(self, 0.0*self.v1,Vector(0,0,0))
assertVectorsAlmostEqual(self, 1.0*self.v1, self.v1)
assertVectorsAlmostEqual(self, (-1)*self.v1, self.v2)
#Test == operator
def test_equal(self):
self.assertEqual(self.v1, Vector(1,2,3))
self.assertNotEqual(self.v1, Vector(1,2,4))
#Test + operator
def test_add(self):
self.assertEqual(self.v1 + self.v2, Vector(0,0,0))
self.assertEqual(self.v1 + self.v3, Vector(3,4,5))
self.assertEqual(self.v2 + self.v3, Vector(1,0,-1))
#Test - operator
def test_subtract(self):
self.assertEqual(self.v1 - self.v2, Vector(2,4,6))
self.assertEqual(self.v2 - self.v1, Vector(-2,-4,-6))
def test_neg(self):
self.assertEqual(-self.v1, Vector(-1,-2,-3))
def test_str(self):
self.assertEqual(str(self.v1), "(1.0 2.0 3.0)")
def test_dotProduct(self):
self.assertEqual(self.v1 * self.v2, -14)
self.assertEqual(self.v2 * self.v1, -14)
self.assertEqual(self.v1 * self.v3, 12)
self.assertEqual(self.v2 * self.v3, -12)
self.assertEqual(self.v5 * self.v6, 28)
def test_scalarMult(self):
self.assertEqual(self.v1 * 3, Vector(3,6,9))
self.assertEqual(self.v3 * 2, Vector(4,4,4))
#Test getting the magnitude of the vector
def test_length(self):
self.assertEqual(self.v1.length(), sqrt(14))
self.assertEqual(self.v3.length(), sqrt(12))
self.assertEqual(self.v5.length(), 5)
self.assertEqual(self.v6.length(), 6)
#Test turning vector into a unit vector
def test_normalize(self):
self.assertEqual(self.v4.normalize(), Vector(1/sqrt(26), -3/sqrt(26), 4/sqrt(26)))
#Test getting the angle between two vectors
def test_angle(self):
self.assertEqual(self.v5.angle(self.v6), acos(14.0/15))
self.assertEqual(self.v6.angle(self.v5), acos(14.0/15))
self.assertEqual(self.v7.angle(self.v8), acos(0.1 * sqrt(5)))
self.assertEqual(self.v8.angle(self.v7), acos(0.1 * sqrt(5)))
def test_crossProduct(self):
self.assertEqual(self.v1.crossProduct(self.v1), Vector(0,0,0))
self.assertEqual(self.v1.crossProduct(self.v3), Vector(-2,4,-2))
#Can only take the cross product with another vector
with self.assertRaises(TypeError):
self.v1.crossProduct(33)
#Test / operator
def test_Division(self):
self.assertEqual(self.v1.__truediv__(2), Vector(0.5,1,1.5))
self.assertEqual(self.v2.__truediv__(2), Vector(-0.5,-1,-1.5))
#Example, could put cleanup code here
def tearDown(self):
pass
