private static void GetDesiredVector(out Vector3D unit, out double length, ChaseOrientation_GetTorqueArgs e, ChaseDirectionType direction)
{
switch (direction)
{
case ChaseDirectionType.Drag_Velocity_Along:
case ChaseDirectionType.Drag_Velocity_AlongIfVelocityAway:
case ChaseDirectionType.Drag_Velocity_AlongIfVelocityToward:
unit = e.AngVelocityAlongUnit;
length = e.AngVelocityAlongLength;
break;
case ChaseDirectionType.Attract_Direction:
unit = e.Rotation.Axis;
length = e.Rotation.Angle;
break;
case ChaseDirectionType.Drag_Velocity_Any:
unit = e.AngVelocityUnit;
length = e.AngVelocityLength;
break;
case ChaseDirectionType.Drag_Velocity_Orth:
unit = e.AngVelocityOrthUnit;
length = e.AngVelocityOrthLength;
break;
default:
throw new ApplicationException("Unknown DirectionType: " + direction.ToString());
}
}
public Vector3D?GetTorque(ChaseOrientation_GetTorqueArgs e)
{
Vector3D unit;
double length;
GetDesiredVector(out unit, out length, e, this.Direction);
if (Math3D.IsNearZero(unit))
{
return(null);
}
double torque = this.Value;
if (this.IsAccel)
{
// f=ma
torque *= e.ItemMass;
}
if (this.IsSpring)
{
torque *= e.Rotation.Angle;
}
if (this.IsDrag)
{
torque *= -length; // negative, because it needs to be a drag force
}
// Gradient %
if (this.Gradient != null)
{
torque *= ChasePoint_Force.GetGradientPercent(e.Rotation.Angle, this.Gradient);
}
return(unit * torque);
}
private static void GetDesiredVector(out Vector3D unit, out double length, ChaseOrientation_GetTorqueArgs e, ChaseDirectionType direction)
{
switch (direction)
{
case ChaseDirectionType.Drag_Velocity_Along:
case ChaseDirectionType.Drag_Velocity_AlongIfVelocityAway:
case ChaseDirectionType.Drag_Velocity_AlongIfVelocityToward:
unit = e.AngVelocityAlongUnit;
length = e.AngVelocityAlongLength;
break;
case ChaseDirectionType.Attract_Direction:
unit = e.Rotation.Axis;
length = e.Rotation.Angle;
break;
case ChaseDirectionType.Drag_Velocity_Any:
unit = e.AngVelocityUnit;
length = e.AngVelocityLength;
break;
case ChaseDirectionType.Drag_Velocity_Orth:
unit = e.AngVelocityOrthUnit;
length = e.AngVelocityOrthLength;
break;
default:
throw new ApplicationException("Unknown DirectionType: " + direction.ToString());
}
}
public Vector3D? GetTorque(ChaseOrientation_GetTorqueArgs e)
{
Vector3D unit;
double length;
GetDesiredVector(out unit, out length, e, this.Direction);
if (Math3D.IsNearZero(unit))
{
return null;
}
double torque = this.Value;
if (this.IsAccel)
{
// f=ma
torque *= e.ItemMass;
}
if (this.IsSpring)
{
torque *= e.Rotation.Angle;
}
if (this.IsDrag)
{
torque *= -length; // negative, because it needs to be a drag force
}
// Gradient %
if (this.Gradient != null)
{
torque *= ChasePoint_Force.GetGradientPercent(e.Rotation.Angle, this.Gradient);
}
return unit * torque;
}
private void PhysicsBody_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
{
// See if there is anything to do
if (_desiredOrientation == null)
{
return;
}
//TODO: Offset
Vector3D current = e.Body.DirectionToWorld(new Vector3D(0, 0, 1));
Quaternion rotation = Math3D.GetRotation(current, _desiredOrientation.Value);
if (rotation.IsIdentity)
{
// Don't set anything. If they are rotating along the allowed axis, then no problem. If they try
// to rotate off that axis, another iteration of this method will rotate back
//e.Body.AngularVelocity = new Vector3D(0, 0, 0);
return;
}
ChaseOrientation_GetTorqueArgs args = new ChaseOrientation_GetTorqueArgs(this.Item, rotation);
Vector3D? torque = null;
// Call each worker
foreach (var worker in this.Torques)
{
Vector3D? localForce = worker.GetTorque(args);
if (localForce != null)
{
if (torque == null)
{
torque = localForce;
}
else
{
torque = torque.Value + localForce.Value;
}
}
}
// Apply the torque
if (torque != null)
{
// Limit if exceeds this.MaxForce
if (this.MaxTorque != null && torque.Value.LengthSquared > this.MaxTorque.Value * this.MaxTorque.Value)
{
torque = torque.Value.ToUnit(false) * this.MaxTorque.Value;
}
// Limit acceleration
if (this.MaxAcceleration != null)
{
double mass = Item.PhysicsBody.Mass;
//f=ma
double accel = torque.Value.Length / mass;
if (accel > this.MaxAcceleration.Value)
{
torque = torque.Value.ToUnit(false) * (this.MaxAcceleration.Value * mass);
}
}
torque = torque.Value * this.Percent;
e.Body.AddTorque(torque.Value);
}
}
private void PhysicsBody_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
{
// See if there is anything to do
if (_desiredOrientation == null)
{
return;
}
//TODO: Offset
Vector3D current = e.Body.DirectionToWorld(new Vector3D(0, 0, 1));
Quaternion rotation = Math3D.GetRotation(current, _desiredOrientation.Value);
if (rotation.IsIdentity)
{
// Don't set anything. If they are rotating along the allowed axis, then no problem. If they try
// to rotate off that axis, another iteration of this method will rotate back
//e.Body.AngularVelocity = new Vector3D(0, 0, 0);
return;
}
ChaseOrientation_GetTorqueArgs args = new ChaseOrientation_GetTorqueArgs(this.Item, rotation);
Vector3D?torque = null;
// Call each worker
foreach (var worker in this.Torques)
{
Vector3D?localForce = worker.GetTorque(args);
if (localForce != null)
{
if (torque == null)
{
torque = localForce;
}
else
{
torque = torque.Value + localForce.Value;
}
}
}
// Apply the torque
if (torque != null)
{
// Limit if exceeds this.MaxForce
if (this.MaxTorque != null && torque.Value.LengthSquared > this.MaxTorque.Value * this.MaxTorque.Value)
{
torque = torque.Value.ToUnit() * this.MaxTorque.Value;
}
// Limit acceleration
if (this.MaxAcceleration != null)
{
double mass = Item.PhysicsBody.Mass;
//f=ma
double accel = torque.Value.Length / mass;
if (accel > this.MaxAcceleration.Value)
{
torque = torque.Value.ToUnit() * (this.MaxAcceleration.Value * mass);
}
}
torque = torque.Value * this.Percent;
e.Body.AddTorque(torque.Value);
}
}
