public static FP DistanceBetweenPointAndLineSegment(ref TSVector2 point, ref TSVector2 start, ref TSVector2 end)
{
if (start == end)
{
return(TSVector2.Distance(point, start));
}
TSVector2 v = TSVector2.Subtract(end, start);
TSVector2 w = TSVector2.Subtract(point, start);
FP c1 = TSVector2.Dot(w, v);
if (c1 <= 0)
{
return(TSVector2.Distance(point, start));
}
FP c2 = TSVector2.Dot(v, v);
if (c2 <= c1)
{
return(TSVector2.Distance(point, end));
}
FP b = c1 / c2;
TSVector2 pointOnLine = TSVector2.Add(start, TSVector2.Multiply(v, b));
return(TSVector2.Distance(point, pointOnLine));
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
TSVector2 tSVector = data.velocities[this._indexA].v;
FP fP = data.velocities[this._indexA].w;
TSVector2 tSVector2 = data.velocities[this._indexB].v;
FP fP2 = data.velocities[this._indexB].w;
TSVector2 tSVector3 = data.velocities[this._indexC].v;
FP fP3 = data.velocities[this._indexC].w;
TSVector2 tSVector4 = data.velocities[this._indexD].v;
FP fP4 = data.velocities[this._indexD].w;
FP fP5 = TSVector2.Dot(this._JvAC, tSVector - tSVector3) + TSVector2.Dot(this._JvBD, tSVector2 - tSVector4);
fP5 += this._JwA * fP - this._JwC * fP3 + (this._JwB * fP2 - this._JwD * fP4);
FP y = -this._mass * fP5;
this._impulse += y;
tSVector += this._mA * y * this._JvAC;
fP += this._iA * y * this._JwA;
tSVector2 += this._mB * y * this._JvBD;
fP2 += this._iB * y * this._JwB;
tSVector3 -= this._mC * y * this._JvAC;
fP3 -= this._iC * y * this._JwC;
tSVector4 -= this._mD * y * this._JvBD;
fP4 -= this._iD * y * this._JwD;
data.velocities[this._indexA].v = tSVector;
data.velocities[this._indexA].w = fP;
data.velocities[this._indexB].v = tSVector2;
data.velocities[this._indexB].w = fP2;
data.velocities[this._indexC].v = tSVector3;
data.velocities[this._indexC].w = fP3;
data.velocities[this._indexD].v = tSVector4;
data.velocities[this._indexD].w = fP4;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
TSVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
TSVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
TSVector2 vpA = vA + MathUtils.Cross(wA, _rA);
TSVector2 vpB = vB + MathUtils.Cross(wB, _rB);
FP Cdot = -TSVector2.Dot(_uA, vpA) - Ratio * TSVector2.Dot(_uB, vpB);
FP impulse = -_mass * Cdot;
_impulse += impulse;
TSVector2 PA = -impulse * _uA;
TSVector2 PB = -Ratio * impulse * _uB;
vA += _invMassA * PA;
wA += _invIA * MathUtils.Cross(_rA, PA);
vB += _invMassB * PB;
wB += _invIB * MathUtils.Cross(_rB, PB);
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
public override FP ComputeSubmergedArea(ref TSVector2 normal, FP offset, ref Transform xf, out TSVector2 sc)
{
sc = TSVector2.zero;
TSVector2 p = MathUtils.Mul(ref xf, Position);
FP l = -(TSVector2.Dot(normal, p) - offset);
if (l < -Radius + Settings.Epsilon)
{
//Completely dry
return(0);
}
if (l > Radius)
{
//Completely wet
sc = p;
return(Settings.Pi * _2radius);
}
//Magic
FP l2 = l * l;
FP area = _2radius * (FP)((TSMath.Asin((l / Radius)) + TSMath.PiOver2) + l * TSMath.Sqrt(_2radius - l2));
// TODO - PORT
//FP com = -2.0f / 3.0f * (FP)Math.Pow(_2radius - l2, 1.5f) / area;
FP com = new FP(-2) / new FP(3) * (FP)Math.Pow((_2radius - l2).AsFloat(), 1.5f) / area;
sc.x = p.x + normal.x * com;
sc.y = p.y + normal.y * com;
return(area);
}
/// <summary>
/// 获取指定方向移动的最佳距离
/// </summary>
/// <param name="sdf"></param>
/// <param name="pos">起始位置</param>
/// <param name="dir">方向</param>
/// <param name="speed">速度</param>
/// <param name="radius">碰撞半径</param>
/// <returns></returns>
public static TSVector2 GetVaildPositionBySDF(this SDFRawData sdf, TSVector2 pos, TSVector2 dir, FP speed, FP radius)
{
TSVector2 newPos = pos + dir * speed;
FP sd = Sample(sdf, newPos);
//距离障碍物太近，不可行走
if (sd < radius)
{
TSVector2 gradient = Gradient(sdf, newPos);
TSVector2 asjustDir = dir - gradient * TSVector2.Dot(gradient, dir);
newPos = pos + asjustDir.normalized * speed;
//多次迭代
for (int i = 0; i < 3; ++i)
{
sd = Sample(sdf, newPos);
if (sd >= radius)
{
break;
}
newPos += Gradient(sdf, newPos) * (radius - sd);
}
//避免往返
if (TSVector2.Dot(newPos - pos, dir) < FP.Zero)
{
newPos = pos;
}
}
return(newPos);
}
public override bool TestPoint(ref Transform transform, ref TSVector2 point)
{
TSVector2 center = transform.p + MathUtils.Mul(transform.q, Position);
TSVector2 d = point - center;
return(TSVector2.Dot(d, d) <= _2radius);
}
public override bool TestPoint(ref Transform transform, ref TSVector2 point)
{
TSVector2 value = transform.p + MathUtils.Mul(transform.q, this.Position);
TSVector2 tSVector = point - value;
return(TSVector2.Dot(tSVector, tSVector) <= this._2radius);
}
public override FP ComputeSubmergedArea(ref TSVector2 normal, FP offset, ref Transform xf, out TSVector2 sc)
{
sc = TSVector2.zero;
TSVector2 tSVector = MathUtils.Mul(ref xf, this.Position);
FP fP = -(TSVector2.Dot(normal, tSVector) - offset);
bool flag = fP < -base.Radius + Settings.Epsilon;
FP result;
if (flag)
{
result = 0;
}
else
{
bool flag2 = fP > base.Radius;
if (flag2)
{
sc = tSVector;
result = Settings.Pi * this._2radius;
}
else
{
FP y = fP * fP;
FP fP2 = this._2radius * (TSMath.Asin(fP / base.Radius) + TSMath.PiOver2 + fP * TSMath.Sqrt(this._2radius - y));
FP y2 = new FP(-2) / new FP(3) * Math.Pow((double)(this._2radius - y).AsFloat(), 1.5) / fP2;
sc.x = tSVector.x + normal.x * y2;
sc.y = tSVector.y + normal.y * y2;
result = fP2;
}
}
return(result);
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
TSVector2 tSVector = data.velocities[this._indexA].v;
FP fP = data.velocities[this._indexA].w;
TSVector2 tSVector2 = data.velocities[this._indexB].v;
FP fP2 = data.velocities[this._indexB].w;
TSVector2 value = tSVector + MathUtils.Cross(fP, this._rA);
TSVector2 value2 = tSVector2 + MathUtils.Cross(fP2, this._rB);
FP fP3 = this._length - this.MaxLength;
FP fP4 = TSVector2.Dot(this._u, value2 - value);
bool flag = fP3 < 0f;
if (flag)
{
fP4 += data.step.inv_dt * fP3;
}
FP fP5 = -this._mass * fP4;
FP impulse = this._impulse;
this._impulse = TSMath.Min(0f, this._impulse + fP5);
fP5 = this._impulse - impulse;
TSVector2 tSVector3 = fP5 * this._u;
tSVector -= this._invMassA * tSVector3;
fP -= this._invIA * MathUtils.Cross(this._rA, tSVector3);
tSVector2 += this._invMassB * tSVector3;
fP2 += this._invIB * MathUtils.Cross(this._rB, tSVector3);
data.velocities[this._indexA].v = tSVector;
data.velocities[this._indexA].w = fP;
data.velocities[this._indexB].v = tSVector2;
data.velocities[this._indexB].w = fP2;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
TSVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
TSVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
// Cdot = dot(u, v + cross(w, r))
TSVector2 vpA = vA + MathUtils.Cross(wA, _rA);
TSVector2 vpB = vB + MathUtils.Cross(wB, _rB);
FP Cdot = TSVector2.Dot(_u, vpB - vpA);
FP impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
TSVector2 P = impulse * _u;
vA -= _invMassA * P;
wA -= _invIA * MathUtils.Cross(_rA, P);
vB += _invMassB * P;
wB += _invIB * MathUtils.Cross(_rB, P);
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
/// <summary>
/// Tests if a point lies on a line segment.
/// </summary>
/// <remarks>Used by method <c>CalculateBeta()</c>.</remarks>
private static bool PointOnLineSegment(TSVector2 start, TSVector2 end, TSVector2 point)
{
TSVector2 segment = end - start;
return(MathUtils.Area(ref start, ref end, ref point) == 0f &&
TSVector2.Dot(point - start, segment) >= 0f &&
TSVector2.Dot(point - end, segment) <= 0f);
}
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 static TSVector2 Move(this SDFMap map, List <DynamicCircle> circles, FP radius, TSVector2 start, TSVector2 dir, FP len, int layerMask = -1)
{
start = map.SDF.WorldToLocal(start);
TSVector2 end = start + dir * len;
RectInt rect = map.SDF.ToRect(start, end);
int externSize = (int)TSMath.Ceiling(radius / map.SDF.Grain);
rect.max += new Vector2Int(externSize, externSize);
moveFilterCache.Clear();
map.FilterToList(moveFilterCache, rect, layerMask);
FP maxStepLen = radius * FP.Half;
int moveStep = (int)TSMath.Ceiling(len / maxStepLen);
TSVector2 result = start;
for (int i = 1; i <= moveStep; ++i)
{
FP moveLen = maxStepLen;
if (i == moveStep)
{
moveLen = len - (moveStep - 1) * maxStepLen;
}
TSVector2 newPos = result + dir * moveLen;
FP sd = map.Sample(newPos, moveFilterCache, circles);
if (sd < radius)
{
TSVector2 gradient = map.Gradient(moveFilterCache, circles, newPos);
TSVector2 asjustDir = dir - gradient * TSVector2.Dot(gradient, dir);
newPos = result + asjustDir.normalized * moveLen;
//多次迭代
for (int j = 0; j < 3; ++j)
{
sd = map.Sample(newPos, moveFilterCache, circles);
if (sd >= radius)
{
break;
}
newPos += map.Gradient(moveFilterCache, circles, newPos) * (radius - sd);
}
//避免往返
if (TSVector2.Dot(newPos - start, dir) < FP.Zero)
{
return(result + map.SDF.Origin);
}
else
{
result = newPos;
}
break;
}
else
{
result = newPos;
}
}
return(result + map.SDF.Origin);
}
protected sealed override void ComputeProperties()
{
FP fP = Settings.Pi * this._2radius;
this.MassData.Area = fP;
this.MassData.Mass = base.Density * fP;
this.MassData.Centroid = this.Position;
this.MassData.Inertia = this.MassData.Mass * (0.5f * this._2radius + TSVector2.Dot(this.Position, this.Position));
}
//旋转的box
public static FP SDOrientedBox(TSVector2 x, TSVector2 c, TSVector2 rot, TSVector2 b)
{
TSVector2 v = x - c;
FP px = TSMath.Abs(TSVector2.Dot(v, rot)); //在box的x轴的投影长度
FP py = TSMath.Abs(TSVector2.Dot(v, new TSVector2(-rot.y, rot.x))); //在box的y轴的投影长度
TSVector2 p = new TSVector2(px, py);
TSVector2 d = p - b;
return(TSVector2.Max(d, TSVector2.zero).sqrMagnitude + TSMath.Min(TSMath.Max(d.x, d.y), FP.Zero));
}
/// <summary>
/// 当球的运动矢量和边的法线点积小于零，证明和该边可能产生碰撞
/// </summary>
/// <param name="segement"></param>
/// <param name="dir"></param>
/// <returns></returns>
public static bool CheckCloseSegement(tableEdge segement, TSVector2 dir)
{
FP result = TSVector2.Dot(dir, segement.normal);
if (result < 0)
{
return(true);
}
return(false);
}
protected override sealed void ComputeProperties()
{
FP area = Settings.Pi * _2radius;
MassData.Area = area;
MassData.Mass = Density * area;
MassData.Centroid = Position;
// inertia about the local origin
MassData.Inertia = MassData.Mass * (0.5f * _2radius + TSVector2.Dot(Position, Position));
}
/** 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 = TSVector2.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 != TSVector2.zero)
{
collisionNormal.Normalize();
var dot = TSVector2.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 = TSVector2.zero;
}
}
}
/// <summary>
/// 检查圆是否靠近某一边
/// </summary>
/// <param name="E1"></param>
/// <param name="E2"></param>
/// <param name="cur_pos"></param>
/// <param name="next_pos"></param>
/// <returns></returns>
public static bool CheckCloseEdge(TSVector2 E1, TSVector2 E2, TSVector2 cur_pos, TSVector2 next_pos)
{
var perpendicular_cur_pos = PointToLineDir(E1, E2, cur_pos);
var perpendicularA_next_pos = PointToLineDir(E1, E2, next_pos);
var dotResult = TSVector2.Dot(perpendicular_cur_pos, perpendicularA_next_pos);
if (perpendicularA_next_pos.magnitude < perpendicular_cur_pos.magnitude || dotResult < 0)//靠近边的条件
{
return(true);
}
return(false);
}
/// <summary>
/// 圆和边的平面动态相交检测（注意是边的平面 并不是线段）
/// </summary>
/// <param name="tedge"></param>
/// <param name="crd"></param>
/// <returns>是否在该段时间内相交</returns>
public static bool CheckCircle_LineContact(tableEdge tedge, CircleRunData crd, ref FP t_percent)
{
//Sc
TSVector2 Sc = PointToLineDir(tedge.start, tedge.end, crd.cur_pos);
//Se
TSVector2 Se = PointToLineDir(tedge.start, tedge.end, crd.next_pos);
TSVector2 Scnormal = Sc.normalized;
TSVector2 Senormal = Se.normalized;
//TSVector2 Scnormal = Sc.normalized;
//TSVector2 Senormal = Se.normalized;
//只有两种结果 同向和 反向
FP result = TSVector2.Dot(Scnormal, Senormal); //1同向,0垂直,-1反向
FP Scnorm = TSMath.Sqrt(TSMath.Abs(TSVector2.Dot(Sc, Sc))); //Sc模
FP Senorm = TSMath.Sqrt(TSMath.Abs(TSVector2.Dot(Se, Se))); //Se模
//FP radius_square = crd.radius * crd.radius;
if (result > 0 && Scnorm > crd.radius && Senorm > crd.radius)//Sc,Se同向，俩圆圆半径大于到直线距离,不相交
{
return(false);
}
else//相交 求t
{
FP S = 0;
if (result > 0)
{
S = Scnorm - Senorm;
}
else
{
S = Scnorm + Senorm;
}
//TSVector2 sce = Sc - Se;
//FP S = TSMath.Sqrt( TSVector2.Dot(sce, sce));
t_percent = (Scnorm - crd.radius) / S;//圆心到达撞击点的距离/圆心经过的总距离 来求出时间占比
if (t_percent > 1)
{
return(false);
Debug.Log("路程百分比大于1,注意!");
}
//if (t_percent < 0)
//{
// if (Detection.CheckCircle_tableEdgeEndContact(crd, tedge, ref t_percent))
// {
// Debug.Log("修正");
// }
//}
return(t_percent >= 0?true:false);
}
}
public static FP Evaluate(int indexA, int indexB, FP t)
{
Transform transform;
SeparationFunction._sweepA.GetTransform(out transform, t);
Transform transform2;
SeparationFunction._sweepB.GetTransform(out transform2, t);
FP result;
switch (SeparationFunction._type)
{
case SeparationFunctionType.Points:
{
TSVector2 v = SeparationFunction._proxyA.Vertices[indexA];
TSVector2 v2 = SeparationFunction._proxyB.Vertices[indexB];
TSVector2 value = MathUtils.Mul(ref transform, v);
TSVector2 value2 = MathUtils.Mul(ref transform2, v2);
FP fP = TSVector2.Dot(value2 - value, SeparationFunction._axis);
result = fP;
break;
}
case SeparationFunctionType.FaceA:
{
TSVector2 value3 = MathUtils.Mul(ref transform.q, SeparationFunction._axis);
TSVector2 value4 = MathUtils.Mul(ref transform, SeparationFunction._localPoint);
TSVector2 v3 = SeparationFunction._proxyB.Vertices[indexB];
TSVector2 value5 = MathUtils.Mul(ref transform2, v3);
FP fP2 = TSVector2.Dot(value5 - value4, value3);
result = fP2;
break;
}
case SeparationFunctionType.FaceB:
{
TSVector2 value6 = MathUtils.Mul(ref transform2.q, SeparationFunction._axis);
TSVector2 value7 = MathUtils.Mul(ref transform2, SeparationFunction._localPoint);
TSVector2 v4 = SeparationFunction._proxyA.Vertices[indexA];
TSVector2 value8 = MathUtils.Mul(ref transform, v4);
FP fP3 = TSVector2.Dot(value8 - value7, value6);
result = fP3;
break;
}
default:
Debug.Assert(false);
result = 0f;
break;
}
return(result);
}
/// <summary>
/// 求圆心到线段的有向距离
/// </summary>
/// <param name="start">线段起点</param>
/// <param name="end">线段终点</param>
/// <param name="circlePos">圆心位置</param>
/// <returns></returns>
public static TSVector2 PointToLineDir(TSVector2 start, TSVector2 end, TSVector2 circlePos)
{
TSVector2 C = circlePos;
TSVector2 A = start;
TSVector2 B = end;
TSVector2 CA = A - C;
TSVector2 AC = C - A;
TSVector2 AB = B - A;
TSVector2 ABnormal = TSVector2.Normalize(AB);
TSVector2 AO = TSVector2.Dot(AC, ABnormal) * ABnormal;
TSVector2 CO = CA + AO;
return(CO);
}
public static FP Evaluate(int indexA, int indexB, FP t)
{
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t);
_sweepB.GetTransform(out xfB, t);
switch (_type)
{
case SeparationFunctionType.Points:
{
TSVector2 localPointA = _proxyA.Vertices[indexA];
TSVector2 localPointB = _proxyB.Vertices[indexB];
TSVector2 pointA = MathUtils.Mul(ref xfA, localPointA);
TSVector2 pointB = MathUtils.Mul(ref xfB, localPointB);
FP separation = TSVector2.Dot(pointB - pointA, _axis);
return(separation);
}
case SeparationFunctionType.FaceA:
{
TSVector2 normal = MathUtils.Mul(ref xfA.q, _axis);
TSVector2 pointA = MathUtils.Mul(ref xfA, _localPoint);
TSVector2 localPointB = _proxyB.Vertices[indexB];
TSVector2 pointB = MathUtils.Mul(ref xfB, localPointB);
FP separation = TSVector2.Dot(pointB - pointA, normal);
return(separation);
}
case SeparationFunctionType.FaceB:
{
TSVector2 normal = MathUtils.Mul(ref xfB.q, _axis);
TSVector2 pointB = MathUtils.Mul(ref xfB, _localPoint);
TSVector2 localPointA = _proxyA.Vertices[indexA];
TSVector2 pointA = MathUtils.Mul(ref xfA, localPointA);
FP separation = TSVector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Debug.Assert(false);
return(0.0f);
}
}
public static void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index, out TSVector2 normal, out TSVector2 point, out FP separation)
{
Debug.Assert(pc.pointCount > 0);
switch (pc.type)
{
case ManifoldType.Circles:
{
TSVector2 pointA = MathUtils.Mul(ref xfA, pc.localPoint);
TSVector2 pointB = MathUtils.Mul(ref xfB, pc.localPoints[0]);
normal = pointB - pointA;
normal.Normalize();
point = 0.5f * (pointA + pointB);
separation = TSVector2.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB;
}
break;
case ManifoldType.FaceA:
{
normal = MathUtils.Mul(xfA.q, pc.localNormal);
TSVector2 planePoint = MathUtils.Mul(ref xfA, pc.localPoint);
TSVector2 clipPoint = MathUtils.Mul(ref xfB, pc.localPoints[index]);
separation = TSVector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
point = clipPoint;
}
break;
case ManifoldType.FaceB:
{
normal = MathUtils.Mul(xfB.q, pc.localNormal);
TSVector2 planePoint = MathUtils.Mul(ref xfB, pc.localPoint);
TSVector2 clipPoint = MathUtils.Mul(ref xfA, pc.localPoints[index]);
separation = TSVector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
point = clipPoint;
// Ensure normal points from A to B
normal = -normal;
}
break;
default:
normal = TSVector2.zero;
point = TSVector2.zero;
separation = 0;
break;
}
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
TSVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
TSVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
TSVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
TSVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
TSVector2 d = (cB - cA) + rB - rA;
TSVector2 ay = MathUtils.Mul(qA, _localYAxis);
FP sAy = MathUtils.Cross(d + rA, ay);
FP sBy = MathUtils.Cross(rB, ay);
FP C = TSVector2.Dot(d, ay);
FP k = _invMassA + _invMassB + _invIA * _sAy * _sAy + _invIB * _sBy * _sBy;
FP impulse;
if (k != 0.0f)
{
impulse = -C / k;
}
else
{
impulse = 0.0f;
}
TSVector2 P = impulse * ay;
FP LA = impulse * sAy;
FP LB = impulse * sBy;
cA -= _invMassA * P;
aA -= _invIA * LA;
cB += _invMassB * P;
aB += _invIB * LB;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(FP.Abs(C) <= Settings.LinearSlop);
}
public override bool TestPoint(ref Transform transform, ref TSVector2 point)
{
TSVector2 pLocal = MathUtils.MulT(transform.q, point - transform.p);
for (int i = 0; i < Vertices.Count; ++i)
{
FP dot = TSVector2.Dot(Normals[i], pLocal - Vertices[i]);
if (dot > 0.0f)
{
return(false);
}
}
return(true);
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FP invMassA = this._invMassA;
FP invMassB = this._invMassB;
FP invIA = this._invIA;
FP invIB = this._invIB;
TSVector2 tSVector = data.velocities[this._indexA].v;
FP fP = data.velocities[this._indexA].w;
TSVector2 tSVector2 = data.velocities[this._indexB].v;
FP fP2 = data.velocities[this._indexB].w;
FP x = TSVector2.Dot(this._ax, tSVector2 - tSVector) + this._sBx * fP2 - this._sAx * fP;
FP fP3 = -this._springMass * (x + this._bias + this._gamma * this._springImpulse);
this._springImpulse += fP3;
TSVector2 value = fP3 * this._ax;
FP y = fP3 * this._sAx;
FP y2 = fP3 * this._sBx;
tSVector -= invMassA * value;
fP -= invIA * y;
tSVector2 += invMassB * value;
fP2 += invIB * y2;
FP y3 = fP2 - fP - this._motorSpeed;
FP y4 = -this._motorMass * y3;
FP motorImpulse = this._motorImpulse;
FP fP4 = data.step.dt * this._maxMotorTorque;
this._motorImpulse = MathUtils.Clamp(this._motorImpulse + y4, -fP4, fP4);
y4 = this._motorImpulse - motorImpulse;
fP -= invIA * y4;
fP2 += invIB * y4;
FP y5 = TSVector2.Dot(this._ay, tSVector2 - tSVector) + this._sBy * fP2 - this._sAy * fP;
FP fP5 = -this._mass * y5;
this._impulse += fP5;
TSVector2 value2 = fP5 * this._ay;
FP y6 = fP5 * this._sAy;
FP y7 = fP5 * this._sBy;
tSVector -= invMassA * value2;
fP -= invIA * y6;
tSVector2 += invMassB * value2;
fP2 += invIB * y7;
data.velocities[this._indexA].v = tSVector;
data.velocities[this._indexA].w = fP;
data.velocities[this._indexB].v = tSVector2;
data.velocities[this._indexB].w = fP2;
}
public TSVector2 GetSupportVertex(TSVector2 direction)
{
int index = 0;
FP y = TSVector2.Dot(this.Vertices[0], direction);
for (int i = 1; i < this.Vertices.Count; i++)
{
FP fP = TSVector2.Dot(this.Vertices[i], direction);
bool flag = fP > y;
if (flag)
{
index = i;
y = fP;
}
}
return(this.Vertices[index]);
}
/// <summary>
/// Get the supporting vertex in the given direction.
/// </summary>
/// <param name="direction">The direction.</param>
/// <returns></returns>
public TSVector2 GetSupportVertex(TSVector2 direction)
{
int bestIndex = 0;
FP bestValue = TSVector2.Dot(Vertices[0], direction);
for (int i = 1; i < Vertices.Count; ++i)
{
FP value = TSVector2.Dot(Vertices[i], direction);
if (value > bestValue)
{
bestIndex = i;
bestValue = value;
}
}
return(Vertices[bestIndex]);
}
public int GetSupport(TSVector2 direction)
{
int result = 0;
FP y = TSVector2.Dot(this.Vertices[0], direction);
for (int i = 1; i < this.Vertices.Count; i++)
{
FP fP = TSVector2.Dot(this.Vertices[i], direction);
bool flag = fP > y;
if (flag)
{
result = i;
y = fP;
}
}
return(result);
}