public RayCastResult(Body body, double hitDistance, Vector3D normal, double intersectParam)
{
_body = body;
_hitDistance = hitDistance;
_normal = normal;
_intersectFactor = intersectParam;
}
public static BoundingBox GetBoundingBox(Body body)
{
if (body is IMesh)
return GeometryHelper.GetBoundingBox(((IMesh)body).GetPoints());
else
return new BoundingBox();
}
private void InitialiseBody(Body parent, Visual3D visual3D)
{
ModelVisual3D model = (visual3D as ModelVisual3D);
if (model != null)
{
Body body = GetBody(model);
if (body != null)
{
body.Initialise(this);
if (body.Joint != null)
{
if (!body.Joint.IsInitialised)
{
if (parent == null)
parent = new NullBody(this);
body.Joint.Initialise(this, parent, body);
}
}
}
// pass the last visual that had a body attached (if body is null then return the parent (which should not be null)
InitialiseBodies(body ?? parent, model.Children);
}
}
private void Body_ApplyForce(Body sender, BodyForceEventArgs e)
{
if (sender != _physicsBody)
{
return;
}
if (!_hasSetInitialVelocity)
{
e.AddImpulse(_initialVelocity, Math3D.GetRandomVector_Spherical(this.Radius * .5d)); // whacking it at an offset, so it has some rotation as well
_hasSetInitialVelocity = true;
// No need to listen to this even anymore
_physicsBody.ApplyForce -= new BodyForceEventHandler(Body_ApplyForce);
}
}
private void Body_ApplyForce(Body sender, BodyForceEventArgs e)
{
if (sender != _physicsBody)
{
return;
}
//e.ElapsedTime
#region Thrusters
Transform3D transform = _physicsBody.Transform; // I just don't want to keep hitting the property
foreach (List<ThrustLine> thrusts in _thrustLines.Values)
{
foreach (ThrustLine thrust in thrusts)
{
thrust.ApplyForce(1d, transform, e);
}
}
#endregion
}
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;
}
public static Matrix3D BodyToWorldMatrix(Body body, Vector3D direction)
{
return BodyToWorldMatrix(body, new Point3D(), direction);
}
public static Matrix3D WorldToBodyMatrix(Body body)
{
Matrix3D result = body.Transform.Value;
result.Invert();
return result;
}
public Joint(World world, Body parent, Body child)
{
Initialise(world, parent, child);
}
public static Point3D GetCenterPos(Body body)
{
BoundingBox box = GetBoundingBox(body);
return box.CenterPos;
}
public void RemoveBody(Body body)
{
body.NewtonBody.ApplyForceAndTorque -= body_ApplyForceAndTorque;
_removed.Add(body);
}
public void AddBody(Body body)
{
body.NewtonBody.ApplyForceAndTorque += body_ApplyForceAndTorque;
}
public static void SetBody(ModelVisual3D visual, Body value)
{
if (visual == null) throw new ArgumentNullException("visual");
visual.SetValue(BodyProperty, value);
}
/// <summary>
/// This is a method I threw in that will enforce that bodies don't fly out farther than they are supposed to
/// NOTE: This must be called from within the ApplyForceAndTorque callback
/// </summary>
private void FixPositionsAndVelocities(Body body, CApplyForceAndTorqueEventArgs e)
{
const double RETURNVELOCITY = 0d;
double ZACCEL = 10;
if (body == null)
{
return;
}
else if (body is TerrianBody3D)
{
return;
}
// Get the center of mass in world coords
Point3D centerMassWorld = body.PositionToWorld(body.CenterOfMass);
// These are all in world coords
//body.NewtonBody.AddForce(force);
//body.NewtonBody.AddTorque(torque);
//body.NewtonBody.AddImpulse(deltaVelocity, positionOnBody);
if (_boundryMin != null) // if min is non null, _boundryMax is also non null. I don't want to waste the processor checking that each frame
{
#region Stay inside bounding box
// Set the velocity going away to zero, apply a force
//Vector3D velocityWorld = body.DirectionToWorld(body.Velocity);
Vector3D velocityWorld = body.Velocity; // already in world coords
bool modifiedVelocity = false;
#region X
if (centerMassWorld.X < _boundryMin.Value.X)
{
if (velocityWorld.X < 0)
{
velocityWorld.X = RETURNVELOCITY;
modifiedVelocity = true;
}
body.NewtonBody.AddForce(new Vector3D(body.Mass * ZACCEL, 0, 0)); // Apply a constant acceleration until it hits zero
}
else if (centerMassWorld.X > _boundryMax.Value.X)
{
if (velocityWorld.X > 0)
{
velocityWorld.X = -RETURNVELOCITY;
modifiedVelocity = true;
}
body.NewtonBody.AddForce(new Vector3D(body.Mass * ZACCEL * -1, 0, 0)); // Apply a constant acceleration until it hits zero
}
#endregion
#region Y
if (centerMassWorld.Y < _boundryMin.Value.Y)
{
if (velocityWorld.Y < 0)
{
velocityWorld.Y = RETURNVELOCITY;
modifiedVelocity = true;
}
body.NewtonBody.AddForce(new Vector3D(0, body.Mass * ZACCEL, 0)); // Apply a constant acceleration until it hits zero
}
else if (centerMassWorld.Y > _boundryMax.Value.Y)
{
if (velocityWorld.Y > 0)
{
velocityWorld.Y = -RETURNVELOCITY;
modifiedVelocity = true;
}
body.NewtonBody.AddForce(new Vector3D(0, body.Mass * ZACCEL * -1, 0)); // Apply a constant acceleration until it hits zero
}
#endregion
#region Z
if (centerMassWorld.Z < _boundryMin.Value.Z)
{
if (velocityWorld.Z < 0)
{
velocityWorld.Z = RETURNVELOCITY;
modifiedVelocity = true;
}
body.NewtonBody.AddForce(new Vector3D(0, 0, body.Mass * ZACCEL)); // Apply a constant acceleration until it hits zero
}
else if (centerMassWorld.Z > _boundryMax.Value.Z)
{
if (velocityWorld.Z > 0)
{
velocityWorld.Z = -RETURNVELOCITY;
modifiedVelocity = true;
}
body.NewtonBody.AddForce(new Vector3D(0, 0, body.Mass * ZACCEL * -1)); // Apply a constant acceleration until it hits zero
}
#endregion
if (modifiedVelocity)
{
//body.Velocity = body.DirectionFromWorld(velocityWorld);
body.Velocity = velocityWorld; // already in world coords
}
#endregion
}
if (_shouldForce2D)
{
if (!body.Override2DEnforcement_Rotation)
{
#region Angular Pos/Vel
//body.NewtonBody.AddTorque(new Vector3D(.01, 0, 0)); // pulls back (front comes up, rear goes down)
//body.NewtonBody.AddTorque(new Vector3D(0, .1, 0)); // creates a roll to the right (left side goes up, right side goes down)
Vector3D angularVelocity = body.Omega; // Omega seems to be angular velocity
bool modifiedAngularVelocity = false;
//const double RESTORETORQUE = 15d; // TODO: look at the inertia tensor for the axis I want to apply torque to, and do and calculate to make a constant accel
const double RESTORETORQUE = .25d; // TODO: look at the inertia tensor for the axis I want to apply torque to, and do and calculate to make a constant accel
double massMatrixLength = body.MassMatrix.m_I.Length;
double restoreTorqueX = RESTORETORQUE * body.MassMatrix.m_Mass * (1 / (body.MassMatrix.m_I.X / massMatrixLength));
double restoreTorqueY = RESTORETORQUE * body.MassMatrix.m_Mass * (1 / (body.MassMatrix.m_I.Y / massMatrixLength));
//double restoreTorqueX = RESTORETORQUE; // pulling values from the mass matrix seemed to cause the engine to corrupt. See if there's a problem casting that structure
//double restoreTorqueY = RESTORETORQUE;
//TODO: Dampen the angluar velocidy if the object is very close to zero and the angular speed is small. Currently, the object has
// a very slight wobble indefinately
#region X
Vector3D fromVect = new Vector3D(1, 0, 0);
Vector3D toVect = body.DirectionToWorld(fromVect);
Vector3D axis;
double radians;
Math3D.GetRotation(out axis, out radians, fromVect, toVect);
if ((axis.Y > 0 && radians > 0) || (axis.Y < 0 && radians < 0))
{
if (angularVelocity.Y > 0)
{
angularVelocity.Y = 0;
modifiedAngularVelocity = true;
}
//body.NewtonBody.AddTorque(new Vector3D(0, -RESTORETORQUE, 0));
//body.NewtonBody.AddTorque(body.DirectionToWorld(new Vector3D(0, -RESTORETORQUE, 0))); // apply torque seems to want world coords
body.NewtonBody.AddTorque(body.DirectionToWorld(new Vector3D(0, -restoreTorqueY * Math.Abs(radians), 0)));
}
else if ((axis.Y > 0 && radians < 0) || (axis.Y < 0 && radians > 0))
{
if (angularVelocity.Y < 0)
{
angularVelocity.Y = 0;
modifiedAngularVelocity = true;
}
//body.NewtonBody.AddTorque(new Vector3D(0, RESTORETORQUE, 0));
body.NewtonBody.AddTorque(body.DirectionToWorld(new Vector3D(0, restoreTorqueY * Math.Abs(radians), 0)));
}
#endregion
#region Y
fromVect = new Vector3D(0, 1, 0);
toVect = body.DirectionToWorld(fromVect);
Math3D.GetRotation(out axis, out radians, fromVect, toVect);
if ((axis.X > 0 && radians > 0) || (axis.X < 0 && radians < 0))
{
if (angularVelocity.X > 0)
{
angularVelocity.X = 0;
modifiedAngularVelocity = true;
}
//body.NewtonBody.AddTorque(new Vector3D(-RESTORETORQUE, 0, 0));
body.NewtonBody.AddTorque(body.DirectionToWorld(new Vector3D(-restoreTorqueX * Math.Abs(radians), 0, 0)));
}
else if ((axis.X > 0 && radians < 0) || (axis.X < 0 && radians > 0))
{
if (angularVelocity.X < 0)
{
angularVelocity.X = 0;
modifiedAngularVelocity = true;
}
//body.NewtonBody.AddTorque(new Vector3D(RESTORETORQUE, 0, 0));
body.NewtonBody.AddTorque(body.DirectionToWorld(new Vector3D(restoreTorqueX * Math.Abs(radians), 0, 0)));
}
#endregion
if (modifiedAngularVelocity)
{
body.Omega = angularVelocity;
}
#endregion
}
if (!body.Override2DEnforcement_Translation)
{
#region Z Pos/Vel
//Vector3D velocityWorld = body.DirectionToWorld(body.Velocity);
Vector3D velocityWorld = body.Velocity; // already in world coords
bool modifiedVelocity = false;
if (centerMassWorld.Z < 0)
{
if (velocityWorld.Z < 0)
{
velocityWorld.Z = 0;
modifiedVelocity = true;
}
body.NewtonBody.AddForce(new Vector3D(0, 0, body.Mass * ZACCEL)); // Apply a constant acceleration until it hits zero
}
else if (centerMassWorld.Z > 0)
{
if (velocityWorld.Z > 0)
{
velocityWorld.Z = 0;
modifiedVelocity = true;
}
body.NewtonBody.AddForce(new Vector3D(0, 0, body.Mass * ZACCEL * -1d)); // Apply a constant acceleration until it hits zero
}
if (modifiedVelocity)
{
//body.Velocity = body.DirectionFromWorld(velocityWorld);
body.Velocity = velocityWorld;
}
#endregion
}
}
}
public static Matrix3D BodyToBodyMatrix(Body fromBody, Body toBody)
{
Matrix3D result = toBody.Transform.Value;
result.Invert();
result.Append(fromBody.Transform.Value);
return result;
}
public void Initialise(World world, Body parent, Body child)
{
if (_isInitialised)
return;
VerifyAccess();
if (world == null) throw new ArgumentNullException("world");
if (parent == null) throw new ArgumentNullException("parent");
parent.VerifyInitialised("Parent");
if (child == null) throw new ArgumentNullException("child");
child.VerifyInitialised("Child");
_world = world;
_parentBody = parent;
_childBody = child;
_joint = OnInitialise();
if (_joint != null)
{
_joint.CollisionState = (int)(CollisionState)this.CollisionState;
if (this.Stiffness != null)
_joint.Stiffness = (float)this.Stiffness;
_isInitialised = true;
AfterInitialise();
}
}
public static Matrix3D BodyToWorldMatrix(Body body)
{
return body.Transform.Value;
}
public static Point3D BodyToWorld(Body body, Point3D point)
{
return body.Transform.Value.Transform(point);
}
public static Matrix3D BodyToWorldMatrix(Body body, Point3D position, Vector3D direction)
{
Matrix3D result = Math3D.CreateZDirectionMatrix(position, direction);
result.Append(body.Transform.Value);
return result;
}
public static Vector3D BodyToWorld(Body body, Vector3D vector)
{
return body.Transform.Value.Transform(vector);
}
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
}
public static Point3D WorldToBody(Body body, Point3D point)
{
Matrix3D matrix = body.Transform.Value;
matrix.Invert();
return matrix.Transform(point);
}
private void Body_ApplyForce(Body sender, BodyForceEventArgs e)
{
// Apply Gravity
if (!_bodyForces.ContainsKey(sender))
{
return;
}
e.AddForce(_bodyForces[sender]);
}
public static Vector3D WorldToBody(Body body, Vector3D vector)
{
Matrix3D matrix = body.Transform.Value;
matrix.Invert();
return matrix.Transform(vector);
}
public BodyForceEventArgs(Body body)
{
_body = body;
}
public static Point3D BodyToBody(Body fromBody, Body toBody, Point3D point)
{
Matrix3D matrix = BodyToBodyMatrix(fromBody, toBody);
return matrix.Transform(point);
}
private void Body_ApplyForce(Body sender, BodyForceEventArgs e)
{
}
public static Vector3D BodyToBody(Body fromBody, Body toBody, Vector3D vector)
{
Matrix3D matrix = BodyToBodyMatrix(fromBody, toBody);
return matrix.Transform(vector);
}
private void Body_ApplyForce(Body sender, BodyForceEventArgs e)
{
if (sender != _physicsBody)
{
return;
}
#region Thrusters
if (_thrustTransform != null && _thrustPercent > 0d)
{
_thruster.ApplyForce(_thrustPercent, _physicsBody.Transform, _thrustTransform, e);
}
#endregion
}
private void InitialiseBodies(Body parent, ICollection<Visual3D> collection)
{
if (collection.Count > 0)
{
Visual3D[] items = new Visual3D[collection.Count];
collection.CopyTo(items, 0);
foreach (Visual3D visual3D in items)
{
InitialiseBody(parent, visual3D);
}
}
}
