private Point3D GetWorldCenterMass(Body body)
{
ConvexBody3D bodyCast = body as ConvexBody3D;
if (bodyCast == null)
{
throw new ApplicationException("Couldn't cast body as a ConvexBody3D: " + body.ToString());
}
//TODO: Put this in a property off of convexbody
//// Get the center of mass in model coords
//Point3D centerMass = bodyCast.CenterOfMass;
//Vector3D retVal = new Vector3D(centerMass.X, centerMass.Y, centerMass.Z);
//// Transform that into world coords
//body.VisualMatrix.Transform(retVal);
return body.VisualMatrix.Transform(bodyCast.CenterOfMass);
// Exit Function
//return retVal;
}
private void Cube_ApplyForce(Body sender, BodyForceEventArgs e)
{
if (_shouldRandomizeVelocities)
{
#region Set Velocities
_didRandomizeVelocities = true;
Vector3D newVelocity = Math3D.GetRandomVector_Spherical(MAXRANDVELOCITY * _randVelMultiplier);
//sender.Velocity.X = newVelocity.X;
//sender.Velocity.Y = newVelocity.Y;
//sender.Velocity.Z = newVelocity.Z;
e.AddImpulse(newVelocity, sender.CenterOfMass.ToVector());
#endregion
}
#region Do Gravity
// Calculate force between the two
//TODO: Calculate these forces in one place and remember the results
Point3D blueCenterWorld = GetWorldCenterMass(_blueCube);
Point3D redCenterWorld = GetWorldCenterMass(_redCube);
Vector3D gravityLink = blueCenterWorld - redCenterWorld;
double force = GRAVITATIONALCONSTANT * (_blueCube.Mass * _redCube.Mass) / gravityLink.LengthSquared;
gravityLink.Normalize();
gravityLink = Vector3D.Multiply(force, gravityLink);
// Apply the force
if (sender == _blueCube)
{
e.AddForce(Vector3D.Multiply(-1d, gravityLink));
}
else if (sender == _redCube)
{
e.AddForce(gravityLink);
}
else
{
MessageBox.Show("Unknown Sender: " + sender.ToString(), this.Title, MessageBoxButton.OK, MessageBoxImage.Error);
return;
}
#endregion
}
