/// <summary>
/// Calculate the velocity and angular velocity of CM based on the velocity and angular
/// velocity of the point on the rigidbody.
/// </summary>
/// <param name="rb">The associated rigidbody</param>
/// <param name="pointPos">The position of the point in the world coordinate</param>
/// <param name="pointVel">Velocity of the point</param>
/// <param name="pointAngVel">Angular velocity of the point</param>
/// <param name="velocityMax">Maximum velocity of the rigidbody allowed</param>
/// <param name="distMaxForFollowingAngVel">If the distance between the point and the rigidbody
/// is greater than this value, rigidbody's angular velocity will not be updated.</param>
protected void FollowPointMotionRb(Rigidbody rb, Vector3 pointPos, Vector3 pointVel,
Vector3 pointAngVel, float velocityMax, float distMaxForFollowingAngVel)
{
if (float.IsNaN(pointVel.magnitude) || float.IsNaN(pointAngVel.magnitude))
{
return;
}
Vector3 velocityCM = PhysicsUtils.FindCMLinearVelFromPoint(rb, pointPos, pointVel, pointAngVel);
if (velocityCM.magnitude > velocityMax)
{
velocityCM = velocityCM.normalized * velocityMax;
}
rb.velocity = velocityCM;
float distance = Vector3.Distance(pointPos, rb.position);
if (distance < distMaxForFollowingAngVel)
{
rb.angularVelocity = pointAngVel;
}
}
