/// <summary>
/// Create a new chainshape from the vertices.
/// </summary>
/// <param name="vertices">The vertices to use. Must contain 2 or more vertices.</param>
/// <param name="createLoop">Set to true to create a closed loop. It connects the first vertice to the last, and automatically adjusts connectivity to create smooth collisions along the chain.</param>
public ChainShape(Vertices vertices, bool createLoop = false)
: base(0)
{
ShapeType = ShapeType.Chain;
_radius = Settings.PolygonRadius;
Debug.Assert(vertices != null && vertices.Count >= 3);
Debug.Assert(vertices[0] != vertices[vertices.Count - 1]); // FPE. See http://www.box2d.org/forum/viewtopic.php?f=4&t=7973&p=35363
for (int i = 1; i < vertices.Count; ++i)
{
TSVector2 v1 = vertices[i - 1];
TSVector2 v2 = vertices[i];
// If the code crashes here, it means your vertices are too close together.
Debug.Assert(TSVector2.DistanceSquared(v1, v2) > Settings.LinearSlop * Settings.LinearSlop);
}
Vertices = new Vertices(vertices);
if (createLoop)
{
Vertices.Add(vertices[0]);
PrevVertex = Vertices[Vertices.Count - 2]; //FPE: We use the properties instead of the private fields here.
NextVertex = Vertices[1]; //FPE: We use the properties instead of the private fields here.
}
}
public static void Initialize(ref Manifold manifold, ref Transform xfA, FP radiusA, ref Transform xfB, FP radiusB, out TSVector2 normal, out FixedArray2 <TSVector2> points)
{
normal = TSVector2.zero;
points = default(FixedArray2 <TSVector2>);
bool flag = manifold.PointCount == 0;
if (!flag)
{
switch (manifold.Type)
{
case ManifoldType.Circles:
{
normal = new TSVector2(1f, 0f);
TSVector2 tSVector = MathUtils.Mul(ref xfA, manifold.LocalPoint);
TSVector2 tSVector2 = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint);
bool flag2 = TSVector2.DistanceSquared(tSVector, tSVector2) > Settings.EpsilonSqr;
if (flag2)
{
normal = tSVector2 - tSVector;
normal.Normalize();
}
TSVector2 value = tSVector + radiusA * normal;
TSVector2 value2 = tSVector2 - radiusB * normal;
points[0] = 0.5f * (value + value2);
break;
}
case ManifoldType.FaceA:
{
normal = MathUtils.Mul(xfA.q, manifold.LocalNormal);
TSVector2 value3 = MathUtils.Mul(ref xfA, manifold.LocalPoint);
for (int i = 0; i < manifold.PointCount; i++)
{
TSVector2 value4 = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint);
TSVector2 value5 = value4 + (radiusA - TSVector2.Dot(value4 - value3, normal)) * normal;
TSVector2 value6 = value4 - radiusB * normal;
points[i] = 0.5f * (value5 + value6);
}
break;
}
case ManifoldType.FaceB:
{
normal = MathUtils.Mul(xfB.q, manifold.LocalNormal);
TSVector2 value7 = MathUtils.Mul(ref xfB, manifold.LocalPoint);
for (int j = 0; j < manifold.PointCount; j++)
{
TSVector2 value8 = MathUtils.Mul(ref xfA, manifold.Points[j].LocalPoint);
TSVector2 value9 = value8 + (radiusB - TSVector2.Dot(value8 - value7, normal)) * normal;
TSVector2 value10 = value8 - radiusA * normal;
points[j] = 0.5f * (value10 + value9);
}
normal = -normal;
break;
}
}
}
}
public ChainShape(Vertices vertices, bool createLoop = false) : base(0)
{
base.ShapeType = ShapeType.Chain;
this._radius = Settings.PolygonRadius;
Debug.Assert(vertices != null && vertices.Count >= 3);
Debug.Assert(vertices[0] != vertices[vertices.Count - 1]);
for (int i = 1; i < vertices.Count; i++)
{
TSVector2 value = vertices[i - 1];
TSVector2 value2 = vertices[i];
Debug.Assert(TSVector2.DistanceSquared(value, value2) > Settings.LinearSlop * Settings.LinearSlop);
}
this.Vertices = new Vertices(vertices);
if (createLoop)
{
this.Vertices.Add(vertices[0]);
this.PrevVertex = this.Vertices[this.Vertices.Count - 2];
this.NextVertex = this.Vertices[1];
}
}
/// <summary>
/// 检测圆是否和线段的端点相交,参考了俩个动态圆的动态相交测试,都是转化为射线和圆的相交测试
/// </summary>
/// <returns></returns>
public static bool CheckCircle_tableEdgeEndContact(CircleRunData runCircle, tableEdge segement, ref FP _percent, ref TSVector2 _nearestPos)
{
TSVector2 cirPos = runCircle.cur_pos;
TSVector2 nearestPos = TSVector2.zero;
//先确定圆的起始位置离哪个端点最近
if (TSVector2.DistanceSquared(runCircle.cur_pos, segement.start) < TSVector2.DistanceSquared(runCircle.cur_pos, segement.end))
{
_nearestPos = nearestPos = segement.start;
}
else
{
_nearestPos = nearestPos = segement.end;
}
//TSVector2 VA = runCircle.next_pos - runCircle.cur_pos;
//TSVector2 VB = staticCircle.next_pos - staticCircle.cur_pos;
//两个运动方向描述为一方运动另一方静止 so
//TSVector2 VAB = VA - VB;//runCircle相对于staticCircle的运动方向pc
TSVector2 VAB = runCircle.next_pos - runCircle.cur_pos; //动态圆射线运动方向
TSVector2 Idir = nearestPos - cirPos; //射线起点到静态圆的方向
//FP Idir_length_square = TSVector2.Dot(Idir, Idir);
FP Idir_length_square = Idir.LengthSquared();
FP static_radius_square = runCircle.radius * runCircle.radius;
if (Idir_length_square < static_radius_square)//射线起点在圆心内部,相交
{
//_percent = calHitInfo();
//_percent = 1;//一开始就相交的
//Debug.Log("射线起点在圆心内部");
return(false);
}
else//射线起点在圆心外部的情况
{
FP a_projvalue = TSVector2.Dot(Idir, VAB.normalized);
if (a_projvalue < 0)//球体位于射线原点的后面 不相交
{
return(false);
}
else
{
FP m_square = Idir_length_square - a_projvalue * a_projvalue; //球心到投影点距离的平方
if (m_square - static_radius_square > 0) //预测不相交
{
return(false);
}
else//有可能有交点，因为有可能距离不够
{
//var t = calHitInfo(Idir, a_projvalue);
FP b_squar = m_square;
FP f = TSMath.Sqrt(static_radius_square - b_squar); //理论上来说 f是开跟后的结果,应该有俩个值？
FP t1 = a_projvalue - f; //碰撞到静态圆所走的路程，总路程是runCircle.cur_pos+VAB*delataTime;
FP t2 = a_projvalue + f;
FP per = 0;
bool isFlag = false;
if (t1 > 0 && t1 - VAB.magnitude < FP.EN8)
{
isFlag = true;
if (VAB.magnitude < 0)
{
Debug.Log("除数不能为0");
}
per = t1 / VAB.magnitude;
}
if (t2 > 0 && t2 - VAB.magnitude < 0)
{
isFlag = true;
if (VAB.magnitude < 0)
{
Debug.Log("除数不能为0");
}
var per2 = t2 / VAB.magnitude;
if (per2 < per)
{
per = per2;
}
}
_percent = per;
if (_percent > 1)
{
Debug.Log("路程百分比大于1,注意!");
}
if (isFlag && _percent < FP.EN4)
{
return(false);
}
return(isFlag);
}
}
}
}
/// <summary>
/// Evaluate the manifold with supplied transforms. This assumes
/// modest motion from the original state. This does not change the
/// point count, impulses, etc. The radii must come from the Shapes
/// that generated the manifold.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="xfA">The transform for A.</param>
/// <param name="radiusA">The radius for A.</param>
/// <param name="xfB">The transform for B.</param>
/// <param name="radiusB">The radius for B.</param>
/// <param name="normal">World vector pointing from A to B</param>
/// <param name="points">Torld contact point (point of intersection).</param>
public static void Initialize(ref Manifold manifold, ref Transform xfA, FP radiusA, ref Transform xfB, FP radiusB, out TSVector2 normal, out FixedArray2 <TSVector2> points)
{
normal = TSVector2.zero;
points = new FixedArray2 <TSVector2>();
if (manifold.PointCount == 0)
{
return;
}
switch (manifold.Type)
{
case ManifoldType.Circles:
{
normal = new TSVector2(1.0f, 0.0f);
TSVector2 pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint);
TSVector2 pointB = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint);
if (TSVector2.DistanceSquared(pointA, pointB) > Settings.EpsilonSqr)
{
normal = pointB - pointA;
normal.Normalize();
}
TSVector2 cA = pointA + radiusA * normal;
TSVector2 cB = pointB - radiusB * normal;
points[0] = 0.5f * (cA + cB);
}
break;
case ManifoldType.FaceA:
{
normal = MathUtils.Mul(xfA.q, manifold.LocalNormal);
TSVector2 planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint);
for (int i = 0; i < manifold.PointCount; ++i)
{
TSVector2 clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint);
TSVector2 cA = clipPoint + (radiusA - TSVector2.Dot(clipPoint - planePoint, normal)) * normal;
TSVector2 cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
}
}
break;
case ManifoldType.FaceB:
{
normal = MathUtils.Mul(xfB.q, manifold.LocalNormal);
TSVector2 planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint);
for (int i = 0; i < manifold.PointCount; ++i)
{
TSVector2 clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint);
TSVector2 cB = clipPoint + (radiusB - TSVector2.Dot(clipPoint - planePoint, normal)) * normal;
TSVector2 cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
}
// Ensure normal points from A to B.
normal = -normal;
}
break;
}
}