Added GetPointVelocityCOM and using it to avoid checking that the body is static twice (#33)

Jolt/Physics/Body/Body.h

@@ -107,8 +107,11 @@ public:
 	/// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity
 	void					SetAngularVelocityClamped(Vec3Arg inAngularVelocity)			{ JPH_ASSERT(!IsStatic()); mMotionProperties->SetAngularVelocityClamped(inAngularVelocity); }
 
+	/// Velocity of point inPoint (in center of mass space, e.g. on the surface of the body) of the body (unit: m/s)
+	inline const Vec3		GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const			{ return !IsStatic()? mMotionProperties->GetLinearVelocity() + mMotionProperties->GetAngularVelocity().Cross(inPointRelativeToCOM) : Vec3::sZero(); }
+
 	/// Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body (unit: m/s)
-	inline const Vec3		GetPointVelocity(Vec3Arg inPoint) const							{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return !IsStatic()? mMotionProperties->GetLinearVelocity() + mMotionProperties->GetAngularVelocity().Cross(inPoint - mPosition) : Vec3::sZero(); }
+	inline const Vec3		GetPointVelocity(Vec3Arg inPoint) const							{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return GetPointVelocityCOM(inPoint - mPosition); }
 
 	/// Add force (unit: N) for the next time step, will be reset after the next call to PhysicsSimulation::Update
 	inline void				AddForce(Vec3Arg inForce)										{ JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mForce) + inForce).StoreFloat3(&mMotionProperties->mForce); }

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -46,8 +46,8 @@ void ContactConstraintManager::WorldContactPoint::CalculateFrictionAndNonPenetra
 	Vec3 r2 = p - inBody2.GetCenterOfMassPosition();
 
 	// Calculate velocity of collision points
-	Vec3 v1 = inBody1.GetLinearVelocity() + inBody1.GetAngularVelocity().Cross(r1); 
-	Vec3 v2 = inBody2.GetLinearVelocity() + inBody2.GetAngularVelocity().Cross(r2);
+	Vec3 v1 = inBody1.GetPointVelocityCOM(r1); 
+	Vec3 v2 = inBody2.GetPointVelocityCOM(r2);
 	Vec3 relative_velocity = v2 - v1;
 	float normal_velocity = relative_velocity.Dot(inWorldSpaceNormal);
 

Jolt/Physics/PhysicsSystem.cpp

@@ -1908,8 +1908,8 @@ void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, Physi
 					Vec3 r2 = ccd_body->mContactPointOn2 - body2.GetCenterOfMassPosition();
 
 					// Calculate velocity of collision points
-					Vec3 v1 = body1.GetLinearVelocity() + body1.GetAngularVelocity().Cross(r1_plus_u); 
-					Vec3 v2 = body2.GetLinearVelocity() + body2.GetAngularVelocity().Cross(r2);
+					Vec3 v1 = body1.GetPointVelocityCOM(r1_plus_u); 
+					Vec3 v2 = body2.GetPointVelocityCOM(r2);
 					Vec3 relative_velocity = v2 - v1;
 					float normal_velocity = relative_velocity.Dot(ccd_body->mContactNormal);