// NH & KEEN - reworked the homing function for better accuracy
private void MomentaryTurnToPositionUpdate(Vec3 turnToPosition)
{
if (targetBody != null)
{
turnToPosition = targetBody.Position;
if (targetShape != null)
{
turnToPosition += (targetShape.Position * targetBody.Rotation);
}
if (Type.HomingPrediction > 0.0f)
{
float flyTime = (turnToPosition - Position).Length() / Type.Velocity;
turnToPosition += targetBody.LinearVelocity * flyTime * Type.HomingPrediction;
}
}
Vec3 diff = (turnToPosition - Position);
Degree horizontalAngle = new Radian(-MathFunctions.ATan(diff.Y, diff.X)).InDegrees();
Degree verticalAngle = new Radian(MathFunctions.ATan(diff.Z, diff.ToVec2().Length())).InDegrees();
Quat collisionRotation = new Angles(0, 0, horizontalAngle).ToQuat();
collisionRotation *= new Angles(0, verticalAngle, 0).ToQuat();
Radian targetAngle = Quat.GetAngle(Rotation, collisionRotation);
float PI = (float)Math.PI;
if (targetAngle > PI)
{
targetAngle = (2 * PI) - targetAngle;
}
Radian maxTurnAbility = Type.HomingCorrection.InRadians() * TickDelta;
if (targetAngle > maxTurnAbility)
{
float relativeCorrection = maxTurnAbility / targetAngle;
Rotation = Quat.Slerp(Rotation, collisionRotation, relativeCorrection);
}
else
{
Rotation = collisionRotation;
}
}
private static DetectionInfo Detect(Chain chain, int startingJoint = 0)
{
IList <Joint> joints = chain.GetJoints();
for (int i = startingJoint; i < joints.Count; i++)
{
if (!joints[i].HasConstraint)
{
continue;
}
Quaternion relative = i > 0 ? Quat.Relative(joints[i].Orientation, joints[i - 1].Orientation) :
Quat.Relative(joints[i].Orientation, joints[i].RestOrientation);
float angle = Quat.GetAngle(relative); // [-180, 180)
float min = -Mathf.Abs(joints[i].MaxAngleCW); // (-180, 0]
float max = Mathf.Abs(joints[i].MaxAngleCCW); // [0, 180)
//Debug.Log("ANGLE: " + angle + " MIN: " + min + " MAX: " + max);
DetectionInfo di = new DetectionInfo()
{
index = i, relative = relative
};
if (angle < min)
{
//Debug.Log("Angle BELOW the minimum");
di.angle = min;
return(di);
}
else if (angle > max)
{
//Debug.Log("Angle ABOVE the maximum");
di.angle = max;
return(di);
}
}
return(null);
}