public Vector3?GetTorque(FollowOrientation_Args e)
{
GetDesiredVector(out Vector3 unit, out float length, e, Direction);
if (unit.IsNearZero())
{
return(null);
}
float torque = Value;
// Since this is copied into unity and inertia is a vector, just add directly to the rigid body as an acceleration
//if (IsAccel)
//{
// // f=ma
// torque *= e.MomentInertia;
//}
if (IsSpring)
{
torque *= e.Rotation_Angle;
}
if (IsDrag)
{
torque *= -length; // negative, because it needs to be a drag force
}
// Gradient %
if (Gradient != null)
{
torque *= GradientEntry.GetGradientPercent(e.Rotation_Angle, Gradient);
}
return(unit * torque);
}
public void Tick()
{
if (_desiredOrientation == null)
{
return;
}
Quaternion rotation = Quaternion.FromToRotation(_body.rotation.ToWorld(_initialDirectionLocal), _desiredOrientation.Value);
if (rotation == Quaternion.identity) // there might be some exotic torque definitions that only care about velocity. Just wait a frame and the diff won't be identity
{
return;
}
var args = new FollowOrientation_Args(_body.angularVelocity, rotation);
// Call each worker
foreach (var worker in _workers)
{
Vector3?localForce = worker.GetTorque(args);
if (localForce == null)
{
continue;
}
ForceMode mode = worker.IsAccel ?
ForceMode.Acceleration :
ForceMode.Force;
_body.AddTorque(localForce.Value * Percent, mode);
}
}
private static void GetDesiredVector(out Vector3 unit, out float length, FollowOrientation_Args e, FollowDirectionType direction)
{
switch (direction)
{
case FollowDirectionType.Drag_Velocity_Along:
unit = e.AngVelocityAlongUnit;
length = e.AngVelocityAlongLength;
break;
case FollowDirectionType.Drag_Velocity_AlongIfVelocityAway:
unit = e.AngVelocityAlongUnit;
length = Vector3.Dot(e.AngVelocityUnit, e.Rotation_Axis) < 0 ?
e.AngVelocityAlongLength :
0f;
break;
case FollowDirectionType.Drag_Velocity_AlongIfVelocityToward:
unit = e.AngVelocityAlongUnit;
length = Vector3.Dot(e.AngVelocityUnit, e.Rotation_Axis) >= 0 ?
e.AngVelocityAlongLength :
0f;
break;
case FollowDirectionType.Attract_Direction:
unit = e.Rotation_Axis;
length = e.Rotation_Angle;
break;
case FollowDirectionType.Drag_Velocity_Any:
unit = e.AngVelocityUnit;
length = e.AngVelocityLength;
break;
case FollowDirectionType.Drag_Velocity_Orth:
unit = e.AngVelocityOrthUnit;
length = e.AngVelocityOrthLength;
break;
default:
throw new ApplicationException($"Unknown DirectionType: {direction}");
}
}
