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