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 } } }
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); } }