Esempio n. 1
0
        /** 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));
            }
        }
Esempio n. 2
0
        // 没有处理在内部的情况
        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);
        }
Esempio n. 3
0
        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);
        }