Exemplo 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));
            }
        }
Exemplo n.º 2
0
            public void Update(IChiselHandles handles, ChiselCylinderDefinition definition)
            {
                var tempBottomDiameterX = definition.BottomDiameterX;
                var tempBottomDiameterZ = definition.isEllipsoid ? definition.BottomDiameterZ : definition.BottomDiameterX;

                float tempTopDiameterX, tempTopDiameterZ;

                if (definition.type == CylinderShapeType.Cone)
                {
                    tempTopDiameterX = 0;
                    tempTopDiameterZ = 0;
                }
                else
                if (definition.type == CylinderShapeType.Cylinder)
                {
                    tempTopDiameterX = tempBottomDiameterX;
                    tempTopDiameterZ = tempBottomDiameterZ;
                }
                else
                {
                    tempTopDiameterX = definition.TopDiameterX;
                    tempTopDiameterZ = definition.isEllipsoid ? definition.TopDiameterZ : definition.TopDiameterX;
                }

                topY    = (definition.height + definition.bottomOffset);
                bottomY = definition.bottomOffset;
                var rotate = Quaternion.AngleAxis(definition.rotation, Vector3.up);

                topXVector    = rotate * Vector3.right * tempTopDiameterX * 0.5f;
                topZVector    = rotate * Vector3.forward * tempTopDiameterZ * 0.5f;
                bottomXVector = rotate * Vector3.right * tempBottomDiameterX * 0.5f;
                bottomZVector = rotate * Vector3.forward * tempBottomDiameterZ * 0.5f;
                normal        = Vector3.up;
                if (topY < bottomY)
                {
                    normal = -Vector3.up;
                }
                else
                {
                    normal = Vector3.up;
                }
                topPoint    = normal * topY;
                bottomPoint = normal * bottomY;


                // Render vertical horizon of cylinder
                // TODO: make this work with math instead of "finding" it
                Vector3     bottomPointA, topPointA;
                Vector3     bottomPointB, topPointB;
                var         camera          = UnityEngine.Camera.current;
                var         cameraTransform = camera.transform;
                var         cameraPosition  = handles.inverseMatrix.MultiplyPoint(cameraTransform.position);
                const float degreeStep      = 5;
                var         pointA          = fullTopCircleHandle.GetPointAtDegree(360 - degreeStep);
                var         pointB          = fullBottomCircleHandle.GetPointAtDegree(360 - degreeStep);
                var         camOrtho        = camera.orthographic;
                var         camForward      = handles.inverseMatrix.MultiplyVector(cameraTransform.forward).normalized;
                var         camDir          = camOrtho ? camForward : (pointA - cameraPosition).normalized;


                var delta   = (pointA - pointB).normalized;
                var normal3 = -Vector3.Cross(delta, Vector3.Cross((pointB - bottomPoint).normalized, delta)).normalized;
                var prevDot = Vector3.Dot(normal3, camDir) < 0;

                bool renderHorizon = false;

                //*
                bottomPointA = Vector3.zero;
                topPointA    = Vector3.zero;
                bottomPointB = Vector3.zero;
                topPointB    = Vector3.zero;
                var lineCount = 0;

                for (float degree = 0; degree < 360; degree += degreeStep)
                {
                    pointA = fullTopCircleHandle.GetPointAtDegree(degree);
                    pointB = fullBottomCircleHandle.GetPointAtDegree(degree);

                    delta   = (pointA - pointB).normalized;
                    normal3 = -Vector3.Cross(delta, Vector3.Cross((pointB - bottomPoint).normalized, delta)).normalized;

                    camDir = camOrtho ? camForward : (pointB - cameraPosition).normalized;
                    var currDot = Vector3.Dot(normal3, camDir) < 0;

                    if (prevDot != currDot)
                    {
                        lineCount++;
                        if (lineCount == 1)
                        {
                            topPointA    = pointA;
                            bottomPointA = pointB;
                        }
                        else
                        //if (lineCount == 2)
                        {
                            topPointB     = pointA;
                            bottomPointB  = pointB;
                            renderHorizon = true;
                            break;
                        }
                    }
                    prevDot = currDot;
                }

    #if false
                {
                    var pointC = (Vector3.right * (definition.topDiameterX * 0.5f)) + (Vector3.up * (definition.height + definition.bottomOffset));
                    var pointD = (Vector3.right * (definition.bottomDiameterX * 0.5f)) + (Vector3.up * definition.bottomOffset);
                    //var deltar      = (pointC - pointD).normalized;
                    //var normala     = -Vector3.Cross(Vector3.forward, deltar).normalized;


                    var DT  = (cameraPosition - topPoint);
                    var DB  = (cameraPosition - bottomPoint);
                    var DmT = DT.magnitude;
                    var DmB = DB.magnitude;
                    //var Dv = D / Dm;

                    var RmT = definition.topDiameterX * 0.5f;
                    var RmB = definition.bottomDiameterX * 0.5f;

                    var cosAT = RmT / DmT;
                    var cosAB = RmB / DmB;
                    var AT    = Mathf.Acos(cosAT) * Mathf.Rad2Deg;
                    var AB    = Mathf.Acos(cosAB) * Mathf.Rad2Deg;
                    var RvT   = (Quaternion.AngleAxis(AT, Vector3.up) * DT).normalized;
                    var RvB   = (Quaternion.AngleAxis(AB, Vector3.up) * DB).normalized;
                    //var R = Rv * Rm;

                    var angleT = Vector3.SignedAngle(Vector3.right, RvT, Vector3.up);
                    var angleB = Vector3.SignedAngle(Vector3.right, RvB, Vector3.up);

                    var arotationT = Quaternion.AngleAxis(angleT, Vector3.up);
                    var arotationB = Quaternion.AngleAxis(angleB, Vector3.up);
                    var ptA        = arotationT * pointC;
                    var ptB        = arotationB * pointD;
                    var prevCol    = handles.color;
                    handles.color = UnityEngine.Color.red;
                    handles.DrawLine(bottomPoint, bottomPoint + Vector3.right);
                    //handles.DrawLine(bottomPoint, bottomPoint + Vector3.forward);
                    //handles.DrawLine(bottomPoint, bottomPoint + normala);
                    //handles.DrawLine(bottomPoint, bottomPoint + deltar);
                    //handles.DrawLine(bottomPoint, bottomPoint + R);
                    handles.DrawLine(bottomPoint, bottomPoint + RvT);
                    handles.DrawLine(bottomPoint, bottomPoint + RvB);
                    //handles.DrawLine(bottomPoint, bottomPoint + desired);
                    handles.DrawLine(ptA, ptB);
                    handles.color = prevCol;
                }
    #endif


                /*/
                 * if (camera.orthographic)
                 * {
                 *  {
                 *      var radius = definition.bottomDiameterX * 0.5f;
                 *      var center = bottomPoint;
                 *      bottomPointA = center + (cameraTransform.right * radius);
                 *      bottomPointB = center - (cameraTransform.right * radius);
                 *  }
                 *  {
                 *      var radius = definition.topDiameterX * 0.5f;
                 *      var center = topPoint;
                 *      topPointA = center + (cameraTransform.right * radius);
                 *      topPointB = center - (cameraTransform.right * radius);
                 *  }
                 * } else
                 * {
                 *  var handleMatrix = handles.matrix;
                 *  renderHorizon = GeometryMath.FindCircleHorizon(handleMatrix, definition.bottomDiameterX, bottomPoint, -normal, out bottomPointB, out bottomPointA);
                 *  renderHorizon = GeometryMath.FindCircleHorizon(handleMatrix, definition.topDiameterX,    topPoint,     normal, out topPointA,    out topPointB) && renderHorizon;
                 *
                 *  if (renderHorizon && definition.bottomDiameterX != definition.topDiameterX)
                 *  {
                 *      renderHorizon = !(GeometryMath.PointInCameraCircle(handleMatrix, bottomPointA, definition.topDiameterX,    topPoint,     normal) ||
                 *                        GeometryMath.PointInCameraCircle(handleMatrix, topPointA,    definition.bottomDiameterX, bottomPoint, -normal) ||
                 *                        GeometryMath.PointInCameraCircle(handleMatrix, bottomPointB, definition.topDiameterX,    topPoint,     normal) ||
                 *                        GeometryMath.PointInCameraCircle(handleMatrix, topPointB,    definition.bottomDiameterX, bottomPoint, -normal));
                 *  }
                 * }
                 * //*/

                if (!renderHorizon)
                {
                    bottomPointA = Vector3.zero;
                    topPointA    = Vector3.zero;
                    bottomPointB = Vector3.zero;
                    topPointB    = Vector3.zero;
                }

                verticalHandle1.From = bottomPointA;
                verticalHandle1.To   = topPointA;
                verticalHandle2.From = bottomPointB;
                verticalHandle2.To   = topPointB;

                fullTopCircleHandle.Center    = topPoint;
                fullBottomCircleHandle.Center = bottomPoint;

                fullTopCircleHandle.DiameterX    = tempTopDiameterX;
                fullTopCircleHandle.DiameterZ    = tempTopDiameterZ;
                fullBottomCircleHandle.DiameterX = tempBottomDiameterX;
                fullBottomCircleHandle.DiameterZ = tempBottomDiameterZ;

                topHandle.Origin    = topPoint;
                bottomHandle.Origin = bottomPoint;


                fullTopCircleHandle.Normal    = normal;
                fullBottomCircleHandle.Normal = -normal;

                topHandle.Normal    = normal;
                bottomHandle.Normal = -normal;


                if (definition.isEllipsoid)
                {
                    if (bottomRadiusHandles == null || bottomRadiusHandles.Length != 4)
                    {
                        bottomRadiusHandles = new IChiselEllipsoidHandle[]
                        {
                            handles.CreateEllipsoidHandle(Vector3.zero, -normal, 0, 0, startAngle: +45f, angles: 90),        // left
                            handles.CreateEllipsoidHandle(Vector3.zero, -normal, 0, 0, startAngle: +45f + 180f, angles: 90), // right
                            handles.CreateEllipsoidHandle(Vector3.zero, -normal, 0, 0, startAngle: -45f, angles: 90),        // forward
                            handles.CreateEllipsoidHandle(Vector3.zero, -normal, 0, 0, startAngle: -45f + 180f, angles: 90), // back
                        };
                    }

                    if (topRadiusHandles == null || topRadiusHandles.Length != 4)
                    {
                        topRadiusHandles = new IChiselEllipsoidHandle[]
                        {
                            handles.CreateEllipsoidHandle(Vector3.zero, normal, 0, 0, startAngle: +45f, angles: 90),          // left
                            handles.CreateEllipsoidHandle(Vector3.zero, normal, 0, 0, startAngle: +45f + 180f, angles: 90),   // right
                            handles.CreateEllipsoidHandle(Vector3.zero, normal, 0, 0, startAngle: -45f, angles: 90),          // forward
                            handles.CreateEllipsoidHandle(Vector3.zero, normal, 0, 0, startAngle: -45f + 180f, angles: 90),   // back
                        };
                    }

                    for (int i = 0; i < bottomRadiusHandles.Length; i++)
                    {
                        bottomRadiusHandles[i].Center    = bottomPoint;
                        bottomRadiusHandles[i].Normal    = -normal;
                        bottomRadiusHandles[i].DiameterX = tempBottomDiameterX;
                        bottomRadiusHandles[i].DiameterZ = tempBottomDiameterZ;
                        bottomRadiusHandles[i].Rotation  = definition.rotation;
                    }

                    for (int i = 0; i < topRadiusHandles.Length; i++)
                    {
                        topRadiusHandles[i].Center    = topPoint;
                        topRadiusHandles[i].Normal    = normal;
                        topRadiusHandles[i].DiameterX = tempTopDiameterX;
                        topRadiusHandles[i].DiameterZ = tempTopDiameterZ;
                        topRadiusHandles[i].Rotation  = definition.rotation;
                    }

                    if (bottomHandles == null || bottomHandles.Length != 4)
                    {
                        bottomHandles = new IChiselHandle[] { bottomHandle, bottomRadiusHandles[0], bottomRadiusHandles[1], bottomRadiusHandles[2], bottomRadiusHandles[3] }
                    }
                    ;
                    if (definition.type != CylinderShapeType.Cone)
                    {
                        if (topHandles == null || topHandles.Length != 5)
                        {
                            topHandles = new IChiselHandle[] { topHandle, topRadiusHandles[0], topRadiusHandles[1], topRadiusHandles[2], topRadiusHandles[3] }
                        }
                        ;
                    }
                    else
                    {
                        if (topHandles == null || topHandles.Length != 1)
                        {
                            topHandles = new IChiselHandle[] { topHandle }
                        }
                        ;
                    }
                }
                else
                {
                    if (bottomRadiusHandles == null || bottomRadiusHandles.Length != 1)
                    {
                        bottomRadiusHandles = new IChiselEllipsoidHandle[] { fullBottomCircleHandle }
                    }
                    ;
                    if (topRadiusHandles == null || topRadiusHandles.Length != 1)
                    {
                        topRadiusHandles = new IChiselEllipsoidHandle[] { fullTopCircleHandle }
                    }
                    ;

                    if (bottomHandles == null || bottomHandles.Length != 2)
                    {
                        bottomHandles = new IChiselHandle[] { bottomHandle, bottomRadiusHandles[0] }
                    }
                    ;
                    if (definition.type != CylinderShapeType.Cone)
                    {
                        if (topHandles == null || topHandles.Length != 2)
                        {
                            topHandles = new IChiselHandle[] { topHandle, topRadiusHandles[0] }
                        }
                        ;
                    }
                    else
                    {
                        if (topHandles == null || topHandles.Length != 1)
                        {
                            topHandles = new IChiselHandle[] { topHandle }
                        }
                        ;
                    }
                }
            }
        }