Dot() публичный статический Метод

public static Dot ( Vector2 a, Vector2 b ) : float
a Vector2
b Vector2
Результат float
Пример #1
0
    public Vector2 GetLongLat(Vector3 point, GameObject gameObject)
    {
        Vector3 dirVector = point - gameObject.transform.position;
        Vector2 v1        = new Vector2(gameObject.transform.forward.x, gameObject.transform.forward.z);
        Vector2 v2        = new Vector2(dirVector.x, dirVector.z);

        //Debug.DrawRay(gameObject.transform.position, dirVector  * 20, Color.blue);

        float xAngle = Mathf.Acos(Vector2.Dot(v1.normalized, v2.normalized)) * Mathf.Rad2Deg;

        v1 = new Vector2(gameObject.transform.forward.y, gameObject.transform.forward.z);
        v2 = new Vector2(dirVector.y, dirVector.z);
        float yAngle = Mathf.Acos(Vector2.Dot(v1.normalized, v2.normalized)) * Mathf.Rad2Deg;

        var loc = gameObject.transform.rotation * point;

        if (loc.y < gameObject.transform.position.y)
        {
            yAngle *= -1.0f;
        }

        if (loc.x > gameObject.transform.position.x)
        {
            xAngle *= -1.0f;
        }

        return(new Vector2(xAngle, yAngle));
    }
Пример #2
0
        protected override Vector3 ClampToNavmesh(Vector3 position, out bool positionChanged)
        {
            if (constrainInsideGraph)
            {
                cachedNNConstraint.tags       = seeker.traversableTags;
                cachedNNConstraint.graphMask  = seeker.graphMask;
                cachedNNConstraint.distanceXZ = true;
                var clampedPosition = PathFindHelper.GetNearest(position, cachedNNConstraint).position;

                // We cannot simply check for equality because some precision may be lost
                // if any coordinate transformations are used.
                var   difference    = movementPlane.ToPlane(clampedPosition - position.ToPFV3());
                float sqrDifference = difference.sqrMagnitude;
                if (sqrDifference > 0.001f * 0.001f)
                {
                    // The agent was outside the navmesh. Remove that component of the velocity
                    // so that the velocity only goes along the direction of the wall, not into it
                    velocity2D -= difference.ToUnityV2() * Vector2.Dot(difference.ToUnityV2(), velocity2D) / sqrDifference;

                    // Make sure the RVO system knows that there was a collision here
                    // Otherwise other agents may think this agent continued
                    // to move forwards and avoidance quality may suffer
                    if (rvoController != null && rvoController.enabled)
                    {
                        rvoController.SetCollisionNormal(difference.ToUnityV2());
                    }
                    positionChanged = true;
                    // Return the new position, but ignore any changes in the y coordinate from the ClampToNavmesh method as the y coordinates in the navmesh are rarely very accurate
                    return(position + movementPlane.ToWorld(difference).ToUnityV3());
                }
            }

            positionChanged = false;
            return(position);
        }
Пример #3
0
        public void SetFacing(Vec2 dir)
        {
            float dot = Vec2.Dot(dir.normalized, Vec2.right);

            if (dot < .65f && dot > -.65f)
            {
                if (dir.y > 0)
                {
                    this.dir = Dir.Up;
                }
                else
                {
                    this.dir = Dir.Down;
                }
            }
            else
            {
                if (dir.x > 0)
                {
                    this.dir = Dir.Right;
                }
                else
                {
                    this.dir = Dir.Left;
                }
            }

            UpdateSkinDir();
            if (carriedItem != null)
            {
                ReconfigureItem();
            }
        }
Пример #4
0
    private void Update()
    {
        var desired = (Enemy.Instance.transform.position - transform.position).normalized;

        PreviewRay(desired, Color.blue);
        desired = AvoidWalls(desired);
        PreviewRay(desired, Color.yellow);
        var anglebetween = Mathf.Acos(Vector3.Dot(desired, heading)) * Mathf.Rad2Deg;
        var maxAngle     = maxAngularSpeed * Time.deltaTime * Mathf.Deg2Rad;

        if (anglebetween <= maxAngle)
        {
            heading = desired.normalized;
        }
        else
        {
            var normal = Vector2.Dot(new Vector2(-heading.y, heading.x), desired);
            if (normal < 0)
            {
                maxAngle *= -1;
            }

            heading =
                new Vector3(
                    heading.x * Mathf.Cos(maxAngle) - heading.y * Mathf.Sin(maxAngle),
                    heading.x * Mathf.Sin(maxAngle) + heading.y * Mathf.Cos(maxAngle))
                .normalized;
        }

        UpdateOrientation();

        transform.position += new Vector3(heading.x, heading.y) * maxSpeed * Time.deltaTime;
    }
Пример #5
0
    public static float DotProductAngle(Vector2 originCord, Vector2 aCord, Vector2 bCord)
    {
        Vector2 aCordChanged    = aCord - originCord;
        Vector2 bCordChanged    = bCord - originCord;
        float   someCombination = (Vector2.Dot(aCordChanged, bCordChanged)) / (aCordChanged.magnitude * bCordChanged.magnitude);

        return(Mathf.Acos(someCombination) * Mathf.Rad2Deg);
    }
Пример #6
0
    private void OnCollisionEnter2D(Collision2D other)
    {
        cols++;
        CheckScored();
        Vector2 normal = other.GetContact(0).normal;
        Vector2 outVel = prevVel - 2 * Vector2.Dot(normal, prevVel) * normal;

        _rb.velocity = outVel.normalized * Mathf.Min(prevVel.magnitude + speedBump, maxSpeed);
    }
Пример #7
0
        /** Reads public properties and stores them in internal fields.
         * This is required because multithreading is used and if another script
         * updated the fields at the same time as this class used them in another thread
         * weird things could happen.
         *
         * Will also set CalculatedTargetPoint and CalculatedSpeed to the result
         * which was last calculated.
         */
        public void BufferSwitch()
        {
            // <== Read public properties
            radius              = Radius;
            height              = Height;
            maxSpeed            = nextMaxSpeed;
            desiredSpeed        = nextDesiredSpeed;
            agentTimeHorizon    = AgentTimeHorizon;
            obstacleTimeHorizon = ObstacleTimeHorizon;
            maxNeighbours       = MaxNeighbours;
            // Manually controlled overrides the agent being locked
            // (if one for some reason uses them at the same time)
            locked              = Locked && !manuallyControlled;
            position            = Position;
            elevationCoordinate = ElevationCoordinate;
            collidesWith        = CollidesWith;
            layer = Layer;

            if (locked)
            {
                // Locked agents do not move at all
                desiredTargetPointInVelocitySpace = position;
                desiredVelocity = currentVelocity = Vector2.zero;
            }
            else
            {
                desiredTargetPointInVelocitySpace = nextTargetPoint - position;

                // Estimate our current velocity
                // This is necessary because other agents need to know
                // how this agent is moving to be able to avoid it
                currentVelocity = (CalculatedTargetPoint - position).normalized * CalculatedSpeed;

                // Calculate the desired velocity from the point we want to reach
                desiredVelocity = desiredTargetPointInVelocitySpace.normalized * desiredSpeed;

                if (collisionNormal != Vector2.zero)
                {
                    collisionNormal.Normalize();
                    var dot = Vector2.Dot(currentVelocity, collisionNormal);

                    // Check if the velocity is going into the wall
                    if (dot < 0)
                    {
                        // If so: remove that component from the velocity
                        currentVelocity -= collisionNormal * dot;
                    }

                    // Clear the normal
                    collisionNormal = Vector2.zero;
                }
            }
        }
Пример #8
0
        void GenerateObstacleVOs(VOBuffer vos)
        {
            var range = maxSpeed * obstacleTimeHorizon;

            // Iterate through all obstacles that we might need to avoid
            for (int i = 0; i < simulator.obstacles.Count; i++)
            {
                var obstacle = simulator.obstacles[i];
                var vertex   = obstacle;
                // Iterate through all edges (defined by vertex and vertex.dir) in the obstacle
                do
                {
                    // Ignore the edge if the agent should not collide with it
                    if (vertex.ignore || (vertex.layer & collidesWith) == 0)
                    {
                        vertex = vertex.next;
                        continue;
                    }

                    // Start and end points of the current segment
                    float elevation1, elevation2;
                    var   p1 = To2D(vertex.position, out elevation1);
                    var   p2 = To2D(vertex.next.position, out elevation2);

                    Vector2 dir = (p2 - p1).normalized;

                    // Signed distance from the line (not segment, lines are infinite)
                    // TODO: Can be optimized
                    float dist = VO.SignedDistanceFromLine(p1, dir, position);

                    if (dist >= -0.01f && dist < range)
                    {
                        float factorAlongSegment = Vector2.Dot(position - p1, p2 - p1) / (p2 - p1).sqrMagnitude;

                        // Calculate the elevation (y) coordinate of the point on the segment closest to the agent
                        var segmentY = Mathf.Lerp(elevation1, elevation2, factorAlongSegment);

                        // Calculate distance from the segment (not line)
                        var sqrDistToSegment = (Vector2.Lerp(p1, p2, factorAlongSegment) - position).sqrMagnitude;

                        // Ignore the segment if it is too far away
                        // or the agent is too high up (or too far down) on the elevation axis (usually y axis) to avoid it.
                        // If the XY plane is used then all elevation checks are disabled
                        if (sqrDistToSegment < range * range && (simulator.movementPlane == MovementPlane.XY || (elevationCoordinate <= segmentY + vertex.height && elevationCoordinate + height >= segmentY)))
                        {
                            vos.Add(VO.SegmentObstacle(p2 - position, p1 - position, Vector2.zero, radius * 0.01f, 1f / ObstacleTimeHorizon, 1f / simulator.DeltaTime));
                        }
                    }

                    vertex = vertex.next;
                } while (vertex != obstacle && vertex != null && vertex.next != null);
            }
        }
Пример #9
0
        /// <summary>
        /// 将incidentBox上的碰撞边数据根据ReferenceBox sideNormal上两条边进行裁剪
        /// </summary>
        /// <param name="vOut">输出的碰撞边数据</param>
        /// <param name="vIn">输入的碰撞边数据</param>
        /// <param name="normal">sideNormal</param>
        /// <param name="offset">裁剪依据边上的点在sideNormal上投影长度</param>
        /// <param name="clipEdge">裁剪依据边编号</param>
        /// <returns></returns>
        static int ClipSegmentToLine(ClipVertex[] vOut, ClipVertex[] vIn, Vec2 normal, float offset, EdgeNumbers clipEdge)
        {
            // Start with no output points
            int numOut = 0;

            // Calculate the distance of end points to the line
            // 如果是正数,说明碰撞边顶点越过了裁剪依据边,需要被裁剪
            float distance0 = Vec2.Dot(normal, vIn[0].v) - offset;
            float distance1 = Vec2.Dot(normal, vIn[1].v) - offset;

            // If the points are behind the plane
            if (distance0 <= 0.0f)
            {
                vOut[numOut++] = vIn[0];
            }
            if (distance1 <= 0.0f)
            {
                vOut[numOut++] = vIn[1];
            }

            // If the points are on different sides of the plane
            if (distance0 * distance1 < 0.0f)
            {
                // Find intersection point of edge and plane
                float interp = distance0 / (distance0 - distance1);
                vOut[numOut].v = vIn[0].v + (vIn[1].v - vIn[0].v) * interp;
                //重新设置碰撞点的特征数据
                //特征数据是一个四元组(in1,out1,in2,out2),被裁剪的顶点由两个Box的两条边形成
                //todo 感觉有bug,顶点顺序保证是逆时针时,判断之后设置特征数据的操作才有意义
                //另一方面,实际不会发生碰撞边被两条sideNormal的边同时裁剪的情况,最多一条
                //在这种情况下,两个裁剪操作中,一个保持原状输出
                //另一个进行了裁剪,顶点顺序可能会发生改变,但这些顶点不会再次进入这个函数
                //所以实际不会出现有bug的情况
                if (distance0 > 0.0f)
                {
                    //(in1,_,_,out2)
                    vOut[numOut].feature         = vIn[0].feature;
                    vOut[numOut].feature.inEdge1 = (char)clipEdge;
                    vOut[numOut].feature.inEdge2 = (char)EdgeNumbers.NO_EDGE;//清空
                }
                else
                {
                    //(_,out1,in2,_)
                    vOut[numOut].feature          = vIn[1].feature;
                    vOut[numOut].feature.outEdge1 = (char)clipEdge;
                    vOut[numOut].feature.outEdge2 = (char)EdgeNumbers.NO_EDGE;//清空
                }
                ++numOut;
            }
            return(numOut);
        }
Пример #10
0
        public bool IntersectsCircle(
            Circle c, bool allowIntersectFromWithin, out float frIntersection)
        {
            frIntersection = -1;

            // Get c in ray space
            c += -start;

            float dot = Vec2.Dot(dir, c.center);

            if (dot > mag + c.radius || dot < -c.radius)
            {
                return(false);
            }

            Vec2  ptNearest = dir * dot;
            float distSq    = (c.center - ptNearest).sqrMagnitude;
            float radSq     = c.radius * c.radius;

            if (distSq > radSq)
            {
                return(false);
            }

            float deltaIntersect = Mathf.Sqrt(radSq - distSq);
            float dotIntersect   = dot - deltaIntersect;

            // not this does not check for circles the ray starts in
            // to do so we would have

            if (dotIntersect >= 0 && dotIntersect <= mag)
            {
                frIntersection = dotIntersect / mag;
                return(true);
            }

            if (allowIntersectFromWithin)
            {
                dotIntersect = dot + deltaIntersect;
                if (dotIntersect >= 0 && dotIntersect <= mag)
                {
                    frIntersection = dotIntersect / mag;
                    return(true);
                }
            }

            return(false);
        }
Пример #11
0
            private bool GetPath()
            {
                var pts = game.GetPath(agent.realPos.Floor(), cfg);

                if (pts == null)
                {
                    return(false);
                }

                path = new LinkedList <Vec2I>(pts);
                var dir = pts[0] - agent.realPos;

                if (pts.Length > 1 && (dir.magnitude < float.Epsilon || Vec2.Dot(dir, pts[1] - pts[0]) < -float.Epsilon))
                {
                    path.RemoveFirst();
                }

                return(true);
            }
Пример #12
0
    void Movement(Vector2 move, bool yMovement)
    {
        float distance = move.magnitude;

        if (distance > minMoveDistance)
        {
            int count = rb2d.Cast(move, contactFilter, hitBuffer, distance + shellRadius);
            hitBufferList.Clear();

            for (int i = 0; i < count; i++)
            {
                hitBufferList.Add(hitBuffer[i]);
            }

            for (int i = 0; i < hitBufferList.Count; i++)
            {
                Vector2 currentNormal = hitBufferList[i].normal;

                if (currentNormal.y > minGroundNormalY)
                {
                    grounded = true;
                    if (yMovement)
                    {
                        groundNormal    = currentNormal;
                        currentNormal.x = 0;
                    }
                }

                float projection = Vector2.Dot(velocity, currentNormal);
                if (projection < 0)
                {
                    velocity = velocity - projection * currentNormal;
                }

                float modifiedDistance = hitBufferList[i].distance - shellRadius;
                distance = modifiedDistance < distance ? modifiedDistance : distance;
            }

            rb2d.position = rb2d.position + move.normalized * distance;
        }
    }
Пример #13
0
        public void ComputeDestinationWeights(float horizontal, float vertical)
        {
            float   totalWeight = 0.0f;
            Vector2 point       = new Vector2(horizontal, vertical);

            foreach (var x in _maps)
            {
                Vector2 pointX        = x.Blend.Position;
                Vector2 pointToPointX = point - pointX;
                float   stateWeight   = 1.0f;

                foreach (var y in _maps)
                {
                    if (x.Blend.Motion.name == y.Blend.Motion.name)
                    {
                        continue;
                    }

                    Vector2 pointY            = y.Blend.Position;
                    Vector2 pointYToPointX    = pointY - pointX;
                    float   pointYToPointXLen = Vector2.Dot(pointYToPointX, pointYToPointX);
                    float   newWeight         = Vector2.Dot(pointToPointX, pointYToPointX) / pointYToPointXLen;
                    newWeight   = 1.0f - newWeight;
                    newWeight   = Mathf.Clamp(newWeight, 0.0f, 1.0f);
                    stateWeight = Mathf.Min(stateWeight, newWeight);
                }

                x.State.destinationWeight = stateWeight;
                totalWeight += stateWeight;
            }

            foreach (var map in _maps)
            {
                map.State.destinationWeight = map.State.destinationWeight / totalWeight;
            }
        }
Пример #14
0
 public static float Angle(Vector2 from, Vector2 to)
 {
     return(Mathf.Acos(Mathf.Clamp(Vector2.Dot(from.normalized, to.normalized), -1f, 1f)) * 57.29578f);
 }
Пример #15
0
 public static Vector2 Reflect(Vector2 inDirection, Vector2 inNormal)
 {
     return(-2f * Vector2.Dot(inNormal, inDirection) * inNormal + inDirection);
 }
Пример #16
0
            /** Gradient and value of the cost function of this VO.
             * The VO has a cost function which is 0 outside the VO
             * and increases inside it as the point moves further into
             * the VO.
             *
             * This is the negative gradient of that function as well as its
             * value (the weight). The negative gradient points in the direction
             * where the function decreases the fastest.
             *
             * The value of the function is the distance to the closest edge
             * of the VO and the gradient is normalized.
             */
            public Vector2 Gradient(Vector2 p, out float weight)
            {
                if (colliding)
                {
                    // Calculate double signed area of the triangle consisting of the points
                    // {line1, line1+dir1, p}
                    float l1 = SignedDistanceFromLine(line1, dir1, p);

                    // Serves as a check for which side of the line the point p is
                    if (l1 >= 0)
                    {
                        weight = l1;
                        return(new Vector2(-dir1.y, dir1.x));
                    }
                    else
                    {
                        weight = 0;
                        return(new Vector2(0, 0));
                    }
                }

                float det3 = SignedDistanceFromLine(cutoffLine, cutoffDir, p);

                if (det3 <= 0)
                {
                    weight = 0;
                    return(Vector2.zero);
                }
                else
                {
                    // Signed distances to the two edges along the sides of the VO
                    float det1 = SignedDistanceFromLine(line1, dir1, p);
                    float det2 = SignedDistanceFromLine(line2, dir2, p);
                    if (det1 >= 0 && det2 >= 0)
                    {
                        // We are inside both of the half planes
                        // (all three if we count the cutoff line)
                        // and thus inside the forbidden region in velocity space

                        // Actually the negative gradient because we want the
                        // direction where it slopes the most downwards, not upwards
                        Vector2 gradient;

                        // Check if we are in the semicircle region near the cap of the VO
                        if (Vector2.Dot(p - line1, dir1) > 0 && Vector2.Dot(p - line2, dir2) < 0)
                        {
                            if (segment)
                            {
                                // This part will only be reached for line obstacles (i.e not other agents)
                                if (det3 < radius)
                                {
                                    PF.Vector3 closestPointOnLine = VectorMath.ClosestPointOnSegment(segmentStart.ToPFV2(), segmentEnd.ToPFV2(), p.ToPFV2());
                                    var        dirFromCenter      = p.ToPFV2() - closestPointOnLine.ToV2();
                                    float      distToCenter;
                                    gradient = VectorMath.Normalize(dirFromCenter, out distToCenter);
                                    // The weight is the distance to the edge
                                    weight = radius - distToCenter;
                                    return(gradient);
                                }
                            }
                            else
                            {
                                var   dirFromCenter = p - circleCenter;
                                float distToCenter;
                                gradient = VectorMath.Normalize(dirFromCenter, out distToCenter);
                                // The weight is the distance to the edge
                                weight = radius - distToCenter;
                                return(gradient);
                            }
                        }

                        if (segment && det3 < det1 && det3 < det2)
                        {
                            weight   = det3;
                            gradient = new Vector2(-cutoffDir.y, cutoffDir.x);
                            return(gradient);
                        }

                        // Just move towards the closest edge
                        // The weight is the distance to the edge
                        if (det1 < det2)
                        {
                            weight   = det1;
                            gradient = new Vector2(-dir1.y, dir1.x);
                        }
                        else
                        {
                            weight   = det2;
                            gradient = new Vector2(-dir2.y, dir2.x);
                        }

                        return(gradient);
                    }

                    weight = 0;
                    return(Vector2.zero);
                }
            }
Пример #17
0
        /// <summary>
        /// 通过轴分离算法得到bodyA和bodyB的碰撞点
        /// </summary>
        /// <param name="contacts"></param>
        /// <param name="bodyA"></param>
        /// <param name="bodyB"></param>
        /// <returns></returns>
        public static int CollideTest(Contact[] contacts, Body bodyA, Body bodyB)
        {
            // Setup
            Vec2 hA = bodyA.m_size * 0.5f;//half size
            Vec2 hB = bodyB.m_size * 0.5f;

            Vec2 posA = bodyA.m_position;
            Vec2 posB = bodyB.m_position;

            Mat22 RotA = new Mat22(bodyA.m_rotation);
            Mat22 RotB = new Mat22(bodyB.m_rotation);

            //对于二维旋转矩阵,矩阵的转置等于矩阵的逆
            Mat22 RotAT = RotA.Transpose();
            Mat22 RotBT = RotB.Transpose();

            Vec2 dp = posB - posA; //全局坐标系中,由BodyA指向BodyB的向量
            Vec2 dA = RotAT * dp;  //BodyA的本地坐标系中,由BodyA指向BodyB的向量
            Vec2 dB = RotBT * dp;  //BodyB的本地坐标系中,由BodyA指向BodyB的向量

            Mat22 C     = RotAT * RotB;
            Mat22 absC  = C.Abs();
            Mat22 absCT = absC.Transpose();

            //确定不相交,提前退出的情形
            // Box A faces
            //absC*hB会得到B在A的本地坐标系中的轴对齐外接矩形的halfSize
            //DebugDraw.Instance.DrawBox(bodyB.m_position, absC * hB * 2, 0, Color.gray);
            Vec2 faceA = dA.Abs() - hA - absC * hB;

            if (faceA.x > 0.0f || faceA.y > 0.0f)
            {
                return(0);
            }

            // Box B faces
            //absCT * hA会得到A在B的本地坐标系中的轴对齐外接矩形的halfSize
            Vec2 faceB = dB.Abs() - absCT * hA - hB;

            if (faceB.x > 0.0f || faceB.y > 0.0f)
            {
                return(0);
            }
            //Step 1
            // Find best axis
            //寻找最小分离轴
            Axis  axis;
            float separation;
            Vec2  normal;

            // Box A faces
            axis       = Axis.FACE_A_X;
            separation = faceA.x;
            //B在A的右方,分离法线是A的X轴方向,由A指向B
            //或者B在A的左方,分离法线是A的-X轴方向,由A指向B
            normal = dA.x > 0.0f ? RotA.col1 : -RotA.col1;

            //const float relativeTol = 0.95f;
            //const float absoluteTol = 0.01f;

            if (faceA.y > separation)
            //if (faceA.y > relativeTol * separation + absoluteTol * hA.y)
            {
                axis       = Axis.FACE_A_Y;
                separation = faceA.y;
                //B在A的上方,分离法线是A的Y轴方向,由A指向B
                //或者B在A的下方,分离法线是A的-Y轴方向,由A指向B
                normal = dA.y > 0.0f ? RotA.col2 : -RotA.col2;
            }

            // Box B faces
            if (faceB.x > separation)
            //if (faceB.x > relativeTol * separation + absoluteTol * hB.x)
            {
                axis       = Axis.FACE_B_X;
                separation = faceB.x;
                //B在A的右方,分离法线是B的X轴方向,由A指向B
                //或者B在A的左方,分离法线是A的-X轴方向,由A指向B
                normal = dB.x > 0.0f ? RotB.col1 : -RotB.col1;
            }

            if (faceB.y > separation)
            //if (faceB.y > relativeTol * separation + absoluteTol * hB.y)
            {
                axis       = Axis.FACE_B_Y;
                separation = faceB.y;
                //B在A的上方,分离法线是B的Y轴方向,由A指向B
                //或者B在A的下方,分离法线是B的-Y轴方向,由A指向B
                normal = dB.y > 0.0f ? RotB.col2 : -RotB.col2;
            }
            // Step 2
            // Setup clipping plane data based on the separating axis
            //根据最小分离轴,决定了一个是referenceBox,另一个是incidentBox
            //获取incident上的碰撞边的数据
            //由referenceBox指向incidnetBox的法线
            Vec2 frontNormal = new Vec2();
            //垂直于frontNormal的方向
            Vec2 sideNormal = new Vec2();

            //incident上的碰撞边数据
            ClipVertex[] incidentEdge = new ClipVertex[2];
            //为了支持将IncidentBox的碰撞边根据referenceBox sideNormal方向上的两条边裁剪的数据
            float front   = 0; //refenceBox的frontNormal方向上的边上的点在frontNormal方向上的投影长度
            float negSide = 0; //refenceBox的sideNormal方向上的边上的点在sideNormal方向上的投影长度
            float posSide = 0; //refenceBox的-sideNormal方向上的边上的点在-sideNormal方向上的投影长度
            //ReferenceBox的两条裁剪边编号
            EdgeNumbers negEdge = EdgeNumbers.NO_EDGE;
            EdgeNumbers posEdge = EdgeNumbers.NO_EDGE;

            // Compute the clipping lines and the line segment to be clipped.
            switch (axis)
            {
            case Axis.FACE_A_X:
            {
                frontNormal = normal;
                front       = Vec2.Dot(posA, frontNormal) + hA.x;
                sideNormal  = RotA.col2;
                float side = Vec2.Dot(posA, sideNormal);
                negSide = -side + hA.y;
                posSide = side + hA.y;
                negEdge = EdgeNumbers.EDGE3;
                posEdge = EdgeNumbers.EDGE1;
                ComputeIncidentEdge(out incidentEdge, hB, posB, RotB, frontNormal);
            }
            break;

            case Axis.FACE_A_Y:
            {
                frontNormal = normal;
                front       = Vec2.Dot(posA, frontNormal) + hA.y;
                sideNormal  = RotA.col1;
                float side = Vec2.Dot(posA, sideNormal);
                negSide = -side + hA.x;
                posSide = side + hA.x;
                negEdge = EdgeNumbers.EDGE2;
                posEdge = EdgeNumbers.EDGE4;
                ComputeIncidentEdge(out incidentEdge, hB, posB, RotB, frontNormal);
            }
            break;

            case  Axis.FACE_B_X:
            {
                frontNormal = -normal;
                front       = Vec2.Dot(posB, frontNormal) + hB.x;
                sideNormal  = RotB.col2;
                float side = Vec2.Dot(posB, sideNormal);
                negSide = -side + hB.y;
                posSide = side + hB.y;
                negEdge = EdgeNumbers.EDGE3;
                posEdge = EdgeNumbers.EDGE1;
                ComputeIncidentEdge(out incidentEdge, hA, posA, RotA, frontNormal);
            }
            break;

            case Axis.FACE_B_Y:
            {
                frontNormal = -normal;
                front       = Vec2.Dot(posB, frontNormal) + hB.y;
                sideNormal  = RotB.col1;
                float side = Vec2.Dot(posB, sideNormal);
                negSide = -side + hB.x;
                posSide = side + hB.x;
                negEdge = EdgeNumbers.EDGE2;
                posEdge = EdgeNumbers.EDGE4;
                ComputeIncidentEdge(out incidentEdge, hA, posA, RotA, frontNormal);
            }
            break;
            }
            foreach (var clipVertex in incidentEdge)
            {
                //DebugDraw.Instance.DrawPoint(clipVertex.v, new Color(1,0,0,0.1f));
            }
            //Step 3
            // clip other face with 5 box planes (1 face plane, 4 edge planes)
            //将incidentBox碰撞边数据根据referenceBox的两个裁剪边进行裁剪
            ClipVertex[] clipPoints1 = new ClipVertex[2];
            ClipVertex[] clipPoints2 = new ClipVertex[2];
            int          np;

            // Clip to negative box side 1
            np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, negSide, negEdge);

            if (np < 2)
            {
                return(0);
            }

            // Clip to positive box side 1
            np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, posSide, posEdge);

            if (np < 2)
            {
                return(0);
            }
            foreach (var clipVertex in clipPoints2)
            {
                //DebugDraw.Instance.DrawPoint(clipVertex.v, new Color(1, 0, 0, 0.5f));
            }
            // Now clipPoints2 contains the clipping points.
            // Due to roundoff, it is possible that clipping removes all points.

            int numContacts = 0;

            for (int i = 0; i < 2; ++i)
            {
                separation = Vec2.Dot(frontNormal, clipPoints2[i].v) - front;

                if (separation <= 0)
                {
                    var contact = contacts[numContacts];
                    contact.m_separation = separation; //是一个负数
                    contact.m_normal     = normal;     //由BodyA指向BodyB
                    // slide contact point onto reference face (easy to cull)
                    //接触点位于参考Box的表面
                    contact.m_position = clipPoints2[i].v - frontNormal * separation;
                    contact.m_feature  = clipPoints2[i].feature;
                    if (axis == Axis.FACE_B_X || axis == Axis.FACE_B_Y)
                    {
                        Flip(ref contact.m_feature);
                    }
                    contacts[numContacts] = contact;
                    ++numContacts;
                }
            }
            return(numContacts);
        }