|
public static Dot ( |
||
| a | ||
| b | ||
| return | float | |
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);
}
