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)); }
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); }
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(); } }
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; }
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); }
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); }
/** 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; } } }
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); } }
/// <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); }
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); }
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); }
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; } }
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; } }
public static float Angle(Vector2 from, Vector2 to) { return(Mathf.Acos(Mathf.Clamp(Vector2.Dot(from.normalized, to.normalized), -1f, 1f)) * 57.29578f); }
public static Vector2 Reflect(Vector2 inDirection, Vector2 inNormal) { return(-2f * Vector2.Dot(inNormal, inDirection) * inNormal + inDirection); }
/** 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); } }
/// <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); }