/** Clamps the velocity to the max speed and optionally the forwards direction.
* \param velocity Desired velocity of the character. In world units per second.
* \param maxSpeed Max speed of the character. In world units per second.
* \param slowdownFactor Value between 0 and 1 which determines how much slower the character should move than normal.
* Normally 1 but should go to 0 when the character approaches the end of the path.
* \param slowWhenNotFacingTarget Prevent the velocity from being too far away from the forward direction of the character
* and slow the character down if the desired velocity is not in the same direction as the forward vector.
* \param forward Forward direction of the character. Used together with the \a slowWhenNotFacingTarget parameter.
*
* Note that all vectors are 2D vectors, not 3D vectors.
*
* \returns The clamped velocity in world units per second.
*/
public static Vector2 ClampVelocity(Vector2 velocity, float maxSpeed, float slowdownFactor, bool slowWhenNotFacingTarget, Vector2 forward)
{
// Max speed to use for this frame
var currentMaxSpeed = maxSpeed * slowdownFactor;
// Check if the agent should slow down in case it is not facing the direction it wants to move in
if (slowWhenNotFacingTarget && (forward.x != 0 || forward.y != 0))
{
float currentSpeed;
var normalizedVelocity = VectorMath.Normalize(velocity.ToPFV2(), out currentSpeed);
float dot = Vector2.Dot(normalizedVelocity.ToUnityV2(), forward);
// Lower the speed when the character's forward direction is not pointing towards the desired velocity
// 1 when velocity is in the same direction as forward
// 0.2 when they point in the opposite directions
float directionSpeedFactor = Mathf.Clamp(dot + 0.707f, 0.2f, 1.0f);
currentMaxSpeed *= directionSpeedFactor;
currentSpeed = Mathf.Min(currentSpeed, currentMaxSpeed);
// Angle between the forwards direction of the character and our desired velocity
float angle = Mathf.Acos(Mathf.Clamp(dot, -1, 1));
// Clamp the angle to 20 degrees
// We cannot keep the velocity exactly in the forwards direction of the character
// because we use the rotation to determine in which direction to rotate and if
// the velocity would always be in the forwards direction of the character then
// the character would never rotate.
// Allow larger angles when near the end of the path to prevent oscillations.
angle = Mathf.Min(angle, (20f + 180f * (1 - slowdownFactor * slowdownFactor)) * Mathf.Deg2Rad);
float sin = Mathf.Sin(angle);
float cos = Mathf.Cos(angle);
// Determine if we should rotate clockwise or counter-clockwise to move towards the current velocity
sin *= Mathf.Sign(normalizedVelocity.x * forward.y - normalizedVelocity.y * forward.x);
// Rotate the #forward vector by #angle radians
// The rotation is done using an inlined rotation matrix.
// See https://en.wikipedia.org/wiki/Rotation_matrix
return(new Vector2(forward.x * cos + forward.y * sin, forward.y * cos - forward.x * sin) * currentSpeed);
}
else
{
return(Vector2.ClampMagnitude(velocity, currentMaxSpeed));
}
}
// 没有处理在内部的情况
public static bool Intersect(CylinderXCollider src, CylinderXCollider dst, out XContact?contact)
{
var space = src.Radius + dst.Radius;
var sqrSpace = space * space;
Vector3 n = Vector3.zero;
n.x = dst.Position.x - src.Position.x;
n.z = dst.Position.z - src.Position.z;
if (n.sqrMagnitude > sqrSpace)
{
contact = null;
return(false);
}
var halfHa = src.Height * 0.5f;
var topA = src.Position.y + halfHa;
var bottomA = src.Position.y - halfHa;
var halfHb = dst.Height * 0.5f;
var topB = dst.Position.y + halfHb;
var bottomB = dst.Position.y - halfHb;
Vector3 normal;
float penetration;
if (Mathf.Sign(topA - topB) == Mathf.Sign(bottomB - bottomA))
{
// 水平相撞
normal = n.normalized;
penetration = space - n.magnitude;
}
else if (n.sqrMagnitude < Mathf.Pow(dst.Radius - src.Radius, 2f))
{
// 垂直相撞
if (dst.Position.y > src.Position.y)
{
normal = Vector3.up;
penetration = topA - bottomB;
}
else
{
normal = Vector3.down;
penetration = topB - bottomA;
}
}
else
{
var horizontalP = space - n.magnitude;
float verticalP;
// 斜向相撞
if (topB > topA)
{
verticalP = topA - bottomB;
}
else
{
verticalP = topB - bottomA;
}
if (horizontalP < verticalP)
{
normal = n.normalized;
penetration = horizontalP;
}
else
{
normal = topB > topA ? Vector3.up : Vector3.down;
penetration = verticalP;
}
}
if (normal == Vector3.zero)
{
normal = Vector3.up;
}
contact = new XContact(src, dst, normal, penetration);
return(true);
}
public static bool Intersect(CubeXCollider src, CylinderXCollider dst, out XContact?contact)
{
var invP = Quaternion.Inverse(src.Quaternion) * (dst.Position - src.Position);
Vector3 n = Vector3.zero;
n.x = invP.x;
n.z = invP.z;
var extents = src.Size * 0.5f;
var halfHa = extents.y;
var topA = halfHa;
var bottomA = -halfHa;
var halfHb = dst.Height * 0.5f;
var topB = invP.y + halfHb;
var bottomB = invP.y - halfHb;
var space = dst.Radius;
var sqrSpace = space * space;
// 相撞时，俯视图下的圆和矩形必然相交
var closest = n;
closest.x = Mathf.Clamp(closest.x, -extents.x, extents.x);
closest.z = Mathf.Clamp(closest.z, -extents.z, extents.z);
if ((n - closest).sqrMagnitude > sqrSpace)
{
contact = null;
return(false);
}
// 处理相交的情况
Vector3 normal;
float penetration;
float verticalP = float.PositiveInfinity;
float horizontalP;
var inside = false;
if (n == closest)
{
inside = true;
var disX = extents.x - Mathf.Abs(n.x);
var disZ = extents.z - Mathf.Abs(n.z);
//找到最近的一个面
if (disX < disZ)
{
// 沿X轴
if (n.x > 0)
{
closest.x = extents.x;
}
else
{
closest.x = -extents.x;
}
}
else
{
// 沿Z轴
if (n.z > 0)
{
closest.z = extents.z;
}
else
{
closest.z = -extents.z;
}
}
horizontalP = space + (n - closest).magnitude;
}
else
{
horizontalP = space - (n - closest).magnitude;
}
if (Mathf.Sign(topA - topB) != Mathf.Sign(bottomB - bottomA))
{
// 斜向相撞
if (topB > topA)
{
verticalP = topA - bottomB;
}
else
{
verticalP = topB - bottomA;
}
}
if (horizontalP < verticalP)
{
normal = (src.Quaternion * (n - closest)).normalized;
if (inside)
{
normal = -normal;
}
penetration = horizontalP;
}
else
{
normal = topB > topA ? Vector3.up : Vector3.down;
penetration = verticalP;
}
if (normal == Vector3.zero)
{
normal = Vector3.up;
}
contact = new XContact(src, dst, normal, penetration);
return(true);
}
