/** 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); }