protected virtual void OnApplyForceAndTorque(CApplyForceAndTorqueEventArgs pEventArgs)
{
if (m_ApplyForceAndTorque != null)
{
m_ApplyForceAndTorque(this, pEventArgs);
}
}
private void body_ApplyForceAndTorque(object sender, CApplyForceAndTorqueEventArgs e)
{
if (ApplyForce != null)
{
ApplyForce(this, new BodyForceEventArgs(this));
}
// Cache velocity
Vector3D prevVelocity = _velocityCached;
_velocityCached = _body.Velocity;
// Cache acceleration
if (_wasAccelerationCachedCalled)
{
double elapsedTime = _world.TimeStep;
if (elapsedTime > 0)
{
_accelerationCached = (_velocityCached - prevVelocity) / elapsedTime;
}
else
{
_accelerationCached = new Vector3D(0, 0, 0);
}
}
}
/// <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
}
}
}
protected virtual void OnApplyForceAndTorque(CApplyForceAndTorqueEventArgs pEventArgs)
{
if (m_ApplyForceAndTorque != null)
{
m_ApplyForceAndTorque(this, pEventArgs);
}
}
private void body_ApplyForceAndTorque(object sender, CApplyForceAndTorqueEventArgs e)
{
CBody newtonBody = (CBody)sender;
Body body = UtilityNewt.GetBodyFromUserData(newtonBody);
bool apply = (body == null) || body.ApplyGravity;
if (apply)
{
newtonBody.AddForce(new Vector3D(0, this.Gravity * newtonBody.MassMatrix.m_Mass, 0));
}
if (body != null)
{
FixPositionsAndVelocities(body, e);
}
}
