/// <summary>
/// Inverses the direction of a vector.
/// </summary>
/// <param name="value">The vector to inverse.</param>
/// <returns>The negated vector.</returns>
public static TSVector Negate(TSVector value)
{
TSVector result;
TSVector.Negate(ref value, out result);
return(result);
}
private void FindSupportPoints(RigidBody body1, RigidBody body2,
Shape shape1, Shape shape2, ref TSVector point, ref TSVector normal,
out TSVector point1, out TSVector point2)
{
TSVector mn;
TSVector.Negate(ref normal, out mn);
TSVector sA;
SupportMapping(body1, shape1, ref mn, out sA);
TSVector sB;
SupportMapping(body2, shape2, ref normal, out sB);
TSVector.Subtract(ref sA, ref point, out sA);
TSVector.Subtract(ref sB, ref point, out sB);
FP dot1 = TSVector.Dot(ref sA, ref normal);
FP dot2 = TSVector.Dot(ref sB, ref normal);
TSVector.Multiply(ref normal, dot1, out sA);
TSVector.Multiply(ref normal, dot2, out sB);
TSVector.Add(ref point, ref sA, out point1);
TSVector.Add(ref point, ref sB, out point2);
}
public override void OnSyncedUpdate()
{
bool goForward = TrueSyncInput.GetBool(FORWARD);
bool goBackward = TrueSyncInput.GetBool(BACKWARD);
bool goRight = TrueSyncInput.GetBool(RIGHT);
bool goLeft = TrueSyncInput.GetBool(LEFT);
if (goForward && !isCollide)
{
tsRigidBody.AddForce(TSVector.forward * forceFactor);
}
if (goBackward && !isCollide)
{
tsRigidBody.AddForce(TSVector.Negate(TSVector.forward) * forceFactor);
}
if (goRight && !isCollide)
{
tsRigidBody.AddForce(TSVector.right * forceFactor);
}
if (goLeft && !isCollide)
{
tsRigidBody.AddForce(TSVector.Negate(TSVector.right) * forceFactor);
}
isCollide = false;
}
private bool OverlapTest(ref CapsuleShape capsule, ref TSVector p1, ref TSVector p2,
ref SphereShape sphere, ref TSVector sphereCenter,
ref TSVector pa, ref TSVector pb, ref TSVector normal, ref FP penetration)
{
SegmentShape cap = SegmentShape.Pool.GetNew();
cap.P1 = p1;
cap.P2 = p2;
TSVector v;
FP r2 = capsule.Radius + sphere.Radius;
r2 *= r2;
FP sb;
cap.ClosestPointTo(ref sphereCenter, out sb, out pb);
SegmentShape.Pool.GiveBack(cap);
TSVector.Subtract(ref sphereCenter, ref pb, out normal);
if (normal.sqrMagnitude - r2 >= TSMath.Epsilon)
{
return(false);
}
penetration = (capsule.radius + sphere.radius) - normal.magnitude;
normal.Normalize();
TSVector.Multiply(ref normal, -sphere.Radius, out v);
TSVector.Add(ref sphereCenter, ref v, out pa);
TSVector.Multiply(ref normal, capsule.Radius, out v);
TSVector.Add(ref pb, ref v, out pb);
TSVector.Negate(ref normal, out normal);
return(true);
}
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
CapsuleShape capsule = this.Shape1 as CapsuleShape;
SphereShape sphere = this.Shape2 as SphereShape;
// Get the center of capsule in world coordinates -> center1
capsule.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of sphere in world coordinates -> center2
sphere.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
TSVector aP1, aP2, axisA;
TSVector halfAxisVecA = TSVector.up * FP.Half * capsule.length;
TSVector.Transform(ref halfAxisVecA, ref orientation1, out axisA);
TSVector.Add(ref center1, ref axisA, out aP1);
TSVector.Negate(ref axisA, out axisA);
TSVector.Add(ref center1, ref axisA, out aP2);
return(OverlapTest(ref capsule, ref aP1, ref aP2, ref sphere, ref center2,
ref point1, ref point2, ref normal, ref penetration));
}
private FP GetSpherePenetration(TSVector boxHalfExtent, TSVector sphereRelPos, ref TSVector closestPoint, out TSVector normal)
{
//project the center of the sphere on the closest face of the box
FP faceDist = boxHalfExtent.x - sphereRelPos.x;
FP minDist = faceDist;
closestPoint.x = boxHalfExtent.x;
normal = TSVector.right;
faceDist = boxHalfExtent.x + sphereRelPos.x;
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.x = -boxHalfExtent.x;
normal = TSVector.Negate(TSVector.right);
}
faceDist = boxHalfExtent.y - sphereRelPos.y;
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.y = boxHalfExtent.y;
normal = TSVector.up;
}
faceDist = boxHalfExtent.y + sphereRelPos.y;
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.y = -boxHalfExtent.y;
normal = TSVector.Negate(TSVector.up);
}
faceDist = boxHalfExtent.z - sphereRelPos.z;
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.z = boxHalfExtent.z;
normal = TSVector.forward;
}
faceDist = boxHalfExtent.z + sphereRelPos.z;
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.z = -boxHalfExtent.z;
normal = TSVector.Negate(TSVector.forward);
}
return(minDist);
}
/// <summary>
/// Gets the axis aligned bounding box of the orientated shape.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref TSMatrix orientation, out TSBBox box)
{
TSMatrix abs; TSMath.Absolute(ref orientation, out abs);
TSVector temp;
TSVector.Transform(ref halfSize, ref abs, out temp);
box.max = temp;
TSVector.Negate(ref temp, out box.min);
}
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
SphereShape sphere1 = this.Shape1 as SphereShape;
SphereShape sphere2 = this.Shape2 as SphereShape;
// Get the center of sphere1 in world coordinates -> center1
sphere1.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of sphere2 in world coordinates -> center2
sphere2.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
TSVector c12 = TSVector.Subtract(center1, center2);
FP dot = TSVector.Dot(c12, c12);
FP r = sphere1.radius + sphere2.radius;
if (dot <= r * r)
{
//Get the unit direction from the first sphere's center to the second sphere's center.
TSVector.Subtract(ref center2, ref center1, out normal);
if (normal.sqrMagnitude < TSMath.Epsilon)
{
// Spheres are on the same position, we can choose any normal vector.
// Possibly it would be better to consider the object movement (velocities), but
// it is not important since this case should be VERY rare.
normal = TSVector.forward;
}
else
{
normal = normal.normalized;
}
FP r1 = sphere1.radius;
FP r2 = sphere2.radius;
point1 = normal * r1 + center1;
point2 = TSVector.Negate(normal) * r2 + center2;
TSVector.Negate(ref normal, out normal);
penetration = r - TSMath.Sqrt(dot);
return(true);
}
return(false);
}
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
CapsuleShape capsule1 = this.Shape1 as CapsuleShape;
CapsuleShape capsule2 = this.Shape2 as CapsuleShape;
// Get the center of box1 in world coordinates -> center1
capsule1.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of box2 in world coordinates -> center2
capsule2.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
TSVector aP1, aP2, axisA;
TSVector halfAxisVecA = TSVector.up * FP.Half * capsule1.length;
TSVector.Transform(ref halfAxisVecA, ref orientation1, out axisA);
TSVector.Add(ref center1, ref axisA, out aP1);
TSVector.Negate(ref axisA, out axisA);
TSVector.Add(ref center1, ref axisA, out aP2);
//UnityEngine.Debug.DrawLine(aP1.ToVector(), aP2.ToVector(), UnityEngine.Color.red, 1);
TSVector bP1, bP2, axisB;
TSVector halfAxisVecB = TSVector.up * FP.Half * capsule2.length;
TSVector.Transform(ref halfAxisVecB, ref orientation2, out axisB);
TSVector.Add(ref center2, ref axisB, out bP1);
TSVector.Negate(ref axisB, out axisB);
TSVector.Add(ref center2, ref axisB, out bP2);
//UnityEngine.Debug.DrawLine(bP1.ToVector(), bP2.ToVector(), UnityEngine.Color.blue, 1);
return(DoOverlapTest(ref capsule1, ref aP1, ref aP2,
ref capsule2, ref bP1, ref bP2, TSVector.zero, ref axisA, ref axisB,
ref point1, ref point2, ref normal, ref penetration));
}
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
SphereShape sphere1 = this.Shape1 as SphereShape;
SphereShape sphere2 = this.Shape2 as SphereShape;
// Get the center of sphere1 in world coordinates -> center1
sphere1.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of sphere2 in world coordinates -> center2
sphere2.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
TSVector c12 = TSVector.Subtract(center1, center2);
FP dot = TSVector.Dot(c12, c12);
FP r = sphere1.radius + sphere2.radius;
if (dot <= r * r)
{
//Get the unit direction from the first sphere's center to the second sphere's center.
TSVector.Subtract(ref center2, ref center1, out normal);
normal = normal.normalized;
FP r1 = sphere1.radius;
FP r2 = sphere2.radius;
point1 = normal * r1 + center1;
point2 = TSVector.Negate(normal) * r2 + center2;
TSVector.Negate(ref normal, out normal);
penetration = r - TSMath.Sqrt(dot);
return(true);
}
return(false);
}
private void CollisionDetected(RigidBody body1, RigidBody body2, TSVector point1, TSVector point2, TSVector normal, FP penetration)
{
bool anyBodyColliderOnly = body1.IsColliderOnly || body2.IsColliderOnly;
Arbiter arbiter = null;
ArbiterMap selectedArbiterMap = null;
if (anyBodyColliderOnly)
{
selectedArbiterMap = arbiterTriggerMap;
}
else
{
selectedArbiterMap = arbiterMap;
}
bool arbiterCreated = false;
lock (selectedArbiterMap) {
selectedArbiterMap.LookUpArbiter(body1, body2, out arbiter);
if (arbiter == null)
{
arbiter = Arbiter.Pool.GetNew();
arbiter.body1 = body1; arbiter.body2 = body2;
selectedArbiterMap.Add(new ArbiterKey(body1, body2), arbiter);
arbiterCreated = true;
}
}
Contact contact = null;
if (arbiter.body1 == body1)
{
TSVector.Negate(ref normal, out normal);
contact = arbiter.AddContact(point1, point2, normal, penetration, contactSettings);
}
else
{
contact = arbiter.AddContact(point2, point1, normal, penetration, contactSettings);
}
if (arbiterCreated)
{
if (anyBodyColliderOnly)
{
/*if (body1.isColliderOnly) {
* events.RaiseTriggerBeginCollide(body1, body2);
* } else {
* events.RaiseTriggerBeginCollide(body2, body1);
* }*/
events.RaiseTriggerBeginCollide(contact);
body1.arbitersTrigger.Add(arbiter);
body2.arbitersTrigger.Add(arbiter);
OverlapPairContact overlapContact = new OverlapPairContact(body1, body2);
overlapContact.contact = contact;
initialTriggers.Add(overlapContact);
}
else
{
events.RaiseBodiesBeginCollide(contact);
addedArbiterQueue.Enqueue(arbiter);
OverlapPairContact overlapContact = new OverlapPairContact(body1, body2);
overlapContact.contact = contact;
initialCollisions.Add(overlapContact);
}
}
else if (contact != null)
{
cacheOverPairContact.SetBodies(body1, body2);
//if (!initialTriggers.Contains(cacheOverPairContact)) Debug.LogWarning("不包含==========");
int cacheOverPairContactHashCode = cacheOverPairContact.GetHashCode();
//cacheOverPairContact.contact = contact;//为什么不在这里这里赋值?因为cacheOverPairContact居然不一定能正确拿到对应在initialTriggers里面的op
//所以只能在下面循环查找出对应的实例化对象
for (int index = 0, length = initialTriggers.Count; index < length; index++)
{
OverlapPairContact op = initialTriggers[index];
if (op.GetHashCode() == cacheOverPairContactHashCode)
{
op.contact = contact;
//Debug.LogErrorFormat("包含3=====:法向量{0},穿透{1},哈希{2}", op.contact.normal.ToString(), op.contact.Penetration, op.GetHashCode());
}
}
}
if (!anyBodyColliderOnly && contact != null)
{
events.RaiseContactCreated(contact);
}
}
private void CollisionDetected(RigidBody body1, RigidBody body2, TSVector point1, TSVector point2, TSVector normal, FP penetration)
{
bool anyBodyColliderOnly = body1.IsColliderOnly || body2.IsColliderOnly;
Arbiter arbiter = null;
ArbiterMap selectedArbiterMap = null;
if (anyBodyColliderOnly)
{
selectedArbiterMap = arbiterTriggerMap;
}
else
{
selectedArbiterMap = arbiterMap;
}
bool arbiterCreated = false;
lock (selectedArbiterMap) {
selectedArbiterMap.LookUpArbiter(body1, body2, out arbiter);
if (arbiter == null)
{
arbiter = Arbiter.Pool.GetNew();
arbiter.body1 = body1; arbiter.body2 = body2;
selectedArbiterMap.Add(new ArbiterKey(body1, body2), arbiter);
arbiterCreated = true;
}
}
Contact contact = null;
if (arbiter.body1 == body1)
{
TSVector.Negate(ref normal, out normal);
contact = arbiter.AddContact(point1, point2, normal, penetration, contactSettings);
}
else
{
contact = arbiter.AddContact(point2, point1, normal, penetration, contactSettings);
}
if (arbiterCreated)
{
if (anyBodyColliderOnly)
{
/*if (body1.isColliderOnly) {
* events.RaiseTriggerBeginCollide(body1, body2);
* } else {
* events.RaiseTriggerBeginCollide(body2, body1);
* }*/
events.RaiseTriggerBeginCollide(contact);
body1.arbitersTrigger.Add(arbiter);
body2.arbitersTrigger.Add(arbiter);
OverlapPairContact overlapContact = new OverlapPairContact(body1, body2);
overlapContact.contact = contact;
initialTriggers.Add(overlapContact);
}
else
{
events.RaiseBodiesBeginCollide(contact);
addedArbiterQueue.Enqueue(arbiter);
OverlapPairContact overlapContact = new OverlapPairContact(body1, body2);
overlapContact.contact = contact;
initialCollisions.Add(overlapContact);
}
}
if (!anyBodyColliderOnly && contact != null)
{
events.RaiseContactCreated(contact);
}
}
private bool DoOverlapTest(ref CapsuleShape a, ref TSVector aP1, ref TSVector aP2,
ref CapsuleShape b, ref TSVector bP1, ref TSVector bP2, TSVector offset, ref TSVector axisA, ref TSVector axisB,
ref TSVector pa, ref TSVector pb, ref TSVector normal, ref FP penetration)
{
capa.P1 = aP1;
capa.P2 = aP2;
TSVector.Add(ref aP1, ref offset, out capa.P1);
TSVector.Add(ref aP2, ref offset, out capa.P2);
capb.P1 = bP1;
capb.P2 = bP2;
TSVector v;
FP sa, sb, r2 = a.Radius + b.Radius;
r2 *= r2;
// find the closest point between the two capsules
SegmentShape.ClosestPoints(ref capa, ref capb, out sa, out pa, out sb, out pb);
TSVector.Subtract(ref pa, ref pb, out normal);
FP sqrPaPb = normal.sqrMagnitude;
if (sqrPaPb - r2 >= TSMath.Epsilon)
{
return(false);
}
else if (sqrPaPb < TSMath.Epsilon)
{
normal = TSVector.forward;
}
else
{
normal.Normalize();
}
TSVector.Negate(ref normal, out normal);
penetration = TSVector.Subtract(pa, pb).magnitude - (a.radius + b.radius);
TSVector.Multiply(ref normal, -a.Radius, out v);
TSVector.Add(ref pa, ref v, out pa);
TSVector.Multiply(ref normal, b.Radius, out v);
TSVector.Add(ref pb, ref v, out pb);
TSVector.Subtract(ref pa, ref offset, out pa);
// if the two capsules are nearly parallel, an additional support point provides stability
if (sa == FP.Zero || sa == FP.One)
{
pa = sa == FP.Zero ? capa.P2 : capa.P1;
capb.ClosestPointTo(ref pa, out sa, out pb);
}
else if (sb == FP.Zero || sb == FP.One)
{
pb = sb == FP.Zero ? capb.P2 : capb.P1;
capa.ClosestPointTo(ref pb, out sb, out pa);
}
else
{
return(true);
}
FP dist = TSVector.Subtract(pa, pb).sqrMagnitude - r2;
penetration = TSVector.Subtract(pa, pb).magnitude - (a.radius + b.radius);
if (dist < TSMath.Epsilon)
{
TSVector.Multiply(ref normal, -a.Radius, out v);
TSVector.Add(ref pa, ref v, out pa);
TSVector.Multiply(ref normal, b.Radius, out v);
TSVector.Add(ref pb, ref v, out pb);
TSVector.Subtract(ref pa, ref offset, out pa);
}
TSVector.Negate(ref normal, out normal);
return(true);
}
public FP GetSphereDistance(ref BoxShape box, ref TSVector boxPosition, ref TSMatrix boxOrientation, ref TSVector sphereCenter, FP radius,
ref TSVector pointOnBox, ref TSVector pointOnSphere, ref TSVector normal)
{
FP margins;
bounds[0] = TSVector.Negate(box.halfSize);
bounds[1] = box.halfSize;
margins = FP.Zero; //TODO: box.Margin; //also add sphereShape margin?
boundsVec[0] = bounds[0];
boundsVec[1] = bounds[1];
TSVector marginsVec = TSVector.one * margins;
// add margins
bounds[0] += marginsVec;
bounds[1] -= marginsVec;
TSVector prel, v3P;
FP sep = FP.MaxValue;
FP sepThis;
// convert point in local space
TSVector.Subtract(ref sphereCenter, ref boxPosition, out sphereCenter);
TSMatrix invBoxOrientation;
TSMatrix.Inverse(ref boxOrientation, out invBoxOrientation);
TSVector.Transform(ref sphereCenter, ref invBoxOrientation, out prel);
bool found = false;
v3P = prel;
for (int i = 0; i < 6; i++)
{
int j = i < 3 ? 0 : 1;
if ((sepThis = (TSVector.Dot(v3P - bounds[j], n[i]))) > FP.Zero)
{
v3P = v3P - n[i] * sepThis;
found = true;
}
}
if (found)
{
bounds[0] = boundsVec[0];
bounds[1] = boundsVec[1];
normal = TSVector.Normalize(prel - v3P);
pointOnBox = v3P + normal * margins;
pointOnSphere = prel - normal * radius;
if ((TSVector.Dot(pointOnSphere - pointOnBox, normal)) > FP.Zero)
{
return(FP.One);
}
FP seps2 = (pointOnBox - pointOnSphere).sqrMagnitude;
//if this fails, fallback into deeper penetration case, below
if (seps2 > TSMath.Epsilon)
{
// transform back in world space
TSVector.Transform(ref pointOnBox, ref boxOrientation, out pointOnBox);
TSVector.Add(ref pointOnBox, ref boxPosition, out pointOnBox);
TSVector.Transform(ref pointOnSphere, ref boxOrientation, out pointOnSphere);
TSVector.Add(ref pointOnSphere, ref boxPosition, out pointOnSphere);
sep = -TSMath.Sqrt(seps2);
normal = (pointOnBox - pointOnSphere);
normal *= FP.One / sep;
}
return(sep);
}
return(FP.One);
}
public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref TSMatrix orientation1,
ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector p1, out TSVector p2, out TSVector normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
p1 = p2 = TSVector.zero;
TSVector r = position1 - position2;
TSVector w, v;
TSVector supVertexA;
TSVector rn, vn;
rn = TSVector.Negate(r);
SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA);
TSVector supVertexB;
SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB);
v = supVertexA - supVertexB;
normal = TSVector.zero;
int maxIter = MaxIterations;
FP distSq = v.sqrMagnitude;
FP epsilon = CollideEpsilon;
while ((distSq > epsilon) && (maxIter-- != 0))
{
vn = TSVector.Negate(v);
SupportMapTransformed(support1, ref orientation1, ref position1, ref vn, out supVertexA);
SupportMapTransformed(support2, ref orientation2, ref position2, ref v, out supVertexB);
w = supVertexA - supVertexB;
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, supVertexA, supVertexB);
}
if (simplexSolver.Closest(out v))
{
distSq = v.sqrMagnitude;
normal = v;
}
else
{
distSq = FP.Zero;
}
}
simplexSolver.ComputePoints(out p1, out p2);
if (normal.sqrMagnitude > TSMath.Epsilon * TSMath.Epsilon)
{
normal.Normalize();
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
//================================技能实现效果相关===========================================
public override void Skill_1(params object[] param) //闪现
{
TSVector rayOrigin = AllTSTransform.position;
TSVector rayDirection = this.Angle;
FP maxDistance = Speed * 50;
//int layerMask = UnityEngine.Physics.DefaultRaycastLayers;
int layerMask = LayerMask.GetMask("Wall");
bool bHit = false;//= TSPhysics.Raycast(rayOrigin, rayDirection, hit, maxDistance, layerMask);
TSRay ray = new TSRay(rayOrigin, rayDirection);
TSRaycastHit hit = PhysicsWorldManager.instance.Raycast(ray, maxDistance, layerMask);
if (hit != null)
{
if (hit.distance <= maxDistance)
{
bHit = true;
}
}
//Debug.LogErrorFormat("闪现1,起点{0},方向{1},距离{2},是否{3}", rayOrigin.ToString(), rayDirection.ToString(), maxDistance.ToString(), bHit);
if (!bHit)
{
AllTSTransform.Translate(Angle * maxDistance);
return;
}
//TrueSyncManager.SyncedInstantiate(WillUsedPrefabs[0], hit.point, RotateTSTransform.rotation);
//Debug.LogFormat("射线碰撞点1,point={0},transform={1},法向量normal={2},collider={3},distance={4}", hit.point.ToString(), hit.transform.ToString(), hit.normal.ToString(), hit.collider.ToString(), hit.distance.ToString());
//开始检查反方向的碰撞点法向量是(0,0,0),说明在在内部
TSVector rayOrigin2 = rayOrigin + (Angle * maxDistance); //rayOrigin+maxDistance的目标坐标
TSVector rayDirection2 = TSVector.Negate(this.Angle); //逆向量的方向
TSRay ray2 = new TSRay(rayOrigin2, rayDirection2);
TSRaycastHit hit2 = PhysicsWorldManager.instance.Raycast(ray2, maxDistance, layerMask);
bool bHit2 = false;
if (hit2 != null)
{
if (hit2.distance <= maxDistance)
{
bHit2 = true;
}
}
//Debug.LogErrorFormat("闪现2,起点{0},方向{1},距离{2},是否{3}", rayOrigin2.ToString(), rayDirection2.ToString(), maxDistance.ToString(), bHit2);
if (bHit2)
{
TrueSyncManager.SyncedInstantiate(WillUsedPrefabs[0], hit2.point, RotateTSTransform.rotation);
//Debug.LogFormat("射线碰撞点2,point={0},transform={1},法向量normal={2},collider={3},distance={4}", hit2.point.ToString(), hit2.transform.ToString(), hit2.normal.ToString(), hit2.collider.ToString(), hit2.distance.ToString());
if (!hit2.normal.IsNearlyZero())
{
if (hit2.distance > (FP)1f)
{
AllTSTransform.Translate(Angle * maxDistance);
//Debug.LogErrorFormat("超距离{0},", hit2.distance.ToString());
}
else
{
AllTSTransform.Translate(Angle * maxDistance);
AllTSTransform.Translate(hit2.normal * ((FP)1f - hit2.distance)); //修正一个人物碰撞半径:碰撞法向量的垂直向量
//Debug.LogErrorFormat("碰撞法向量的垂直向量{0},",(new TSVector(-hit2.normal.z, FP.Zero, hit2.normal.x) * (FP)1f).ToString());
}
return;
}
else
{
//Debug.LogErrorFormat("IsNearlyZero=={0},{1},", hit2.normal.IsNearlyZero(), hit2.normal.sqrMagnitude);
}
}
AllTSTransform.position = hit.point;
AllTSTransform.Translate(hit.normal * (FP)1f); //修正一个人物碰撞半径:碰撞法向量的垂直向量
}
/// <summary>
/// Checks two shapes for collisions.
/// 检查碰撞的两种形状。
/// </summary>
/// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
/// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
/// <param name="orientation1">The orientation of the first shape.</param>
/// <param name="orientation2">The orientation of the second shape.</param>
/// <param name="position1">The position of the first shape.</param>
/// <param name="position2">The position of the second shape</param>
/// <param name="point">The pointin world coordinates, where collision occur.</param>
/// <param name="normal">The normal pointing from body2 to body1.</param>
/// <param name="penetration">Estimated penetration depth of the collision.</param>
/// <returns>Returns true if there is a collision, false otherwise.如果发生冲突，返回true，否则为false </returns>
public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref TSMatrix orientation1,
ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector normal, out FP penetration)
{
// Used variables
TSVector temp1, temp2;
TSVector v01, v02, v0;
TSVector v11, v12, v1;
TSVector v21, v22, v2;
TSVector v31, v32, v3;
TSVector v41 = TSVector.zero, v42 = TSVector.zero, v4 = TSVector.zero;
TSVector mn;
// Initialization of the output
point = normal = TSVector.zero;
penetration = FP.Zero;
//JVector right = JVector.Right;
// Get the center of shape1 in world coordinates -> v01
support1.SupportCenter(out v01);
TSVector.Transform(ref v01, ref orientation1, out v01);
TSVector.Add(ref position1, ref v01, out v01);
// Get the center of shape2 in world coordinates -> v02
support2.SupportCenter(out v02);
TSVector.Transform(ref v02, ref orientation2, out v02);
TSVector.Add(ref position2, ref v02, out v02);
// v0 is the center of the minkowski difference
TSVector.Subtract(ref v02, ref v01, out v0);
// Avoid case where centers overlap -- any direction is fine in this case
if (v0.IsNearlyZero())
{
v0 = new TSVector(FP.EN4, 0, 0);
}
// v1 = support in direction of origin
mn = v0;
TSVector.Negate(ref v0, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
TSVector.Subtract(ref v12, ref v11, out v1);
if (TSVector.Dot(ref v1, ref normal) <= FP.Zero)
{
return(false);
}
// v2 = support perpendicular to v1,v0
TSVector.Cross(ref v1, ref v0, out normal);
if (normal.IsNearlyZero())
{
TSVector.Subtract(ref v1, ref v0, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
normal.Normalize();
point = v11;
TSVector.Add(ref point, ref v12, out point);
TSVector.Multiply(ref point, FP.Half, out point);
TSVector.Subtract(ref v12, ref v11, out temp1);
penetration = TSVector.Dot(ref temp1, ref normal);
//point = v11;
//point2 = v12;
return(true);
}
TSVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
TSVector.Subtract(ref v22, ref v21, out v2);
if (TSVector.Dot(ref v2, ref normal) <= FP.Zero)
{
return(false);
}
// Determine whether origin is on + or - side of plane (v1,v0,v2)
TSVector.Subtract(ref v1, ref v0, out temp1);
TSVector.Subtract(ref v2, ref v0, out temp2);
TSVector.Cross(ref temp1, ref temp2, out normal);
FP dist = TSVector.Dot(ref normal, ref v0);
// If the origin is on the - side of the plane, reverse the direction of the plane
if (dist > FP.Zero)
{
TSVector.Swap(ref v1, ref v2);
TSVector.Swap(ref v11, ref v21);
TSVector.Swap(ref v12, ref v22);
TSVector.Negate(ref normal, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
}
int phase2 = 0;
int phase1 = 0;
bool hit = false;
// Phase One: Identify a portal
while (true)
{
if (phase1 > MaximumIterations)
{
return(false);
}
phase1++;
// Obtain the support point in a direction perpendicular to the existing plane
// Note: This point is guaranteed to lie off the plane
TSVector.Negate(ref normal, out mn);
//UnityEngine.Debug.Log("mn: " + mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
TSVector.Subtract(ref v32, ref v31, out v3);
if (TSVector.Dot(ref v3, ref normal) <= FP.Zero)
{
return(false);
}
// If origin is outside (v1,v0,v3), then eliminate v2 and loop
TSVector.Cross(ref v1, ref v3, out temp1);
if (TSVector.Dot(ref temp1, ref v0) < FP.Zero)
{
v2 = v3;
v21 = v31;
v22 = v32;
TSVector.Subtract(ref v1, ref v0, out temp1);
TSVector.Subtract(ref v3, ref v0, out temp2);
TSVector.Cross(ref temp1, ref temp2, out normal);
// UnityEngine.Debug.Log("normal: " + normal);
continue;
}
// If origin is outside (v3,v0,v2), then eliminate v1 and loop
TSVector.Cross(ref v3, ref v2, out temp1);
if (TSVector.Dot(ref temp1, ref v0) < FP.Zero)
{
v1 = v3;
v11 = v31;
v12 = v32;
TSVector.Subtract(ref v3, ref v0, out temp1);
TSVector.Subtract(ref v2, ref v0, out temp2);
TSVector.Cross(ref temp1, ref temp2, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
continue;
}
// Phase Two: Refine the portal
// We are now inside of a wedge...
while (true)
{
phase2++;
/*
* UnityEngine.Debug.LogError(" ::Start STATE");
* UnityEngine.Debug.Log(temp1 + " " + temp2);
* UnityEngine.Debug.Log( v01 + " " + v02 + " "+ v0);
* UnityEngine.Debug.Log( v11+" "+ v12 +" "+ v1);
* UnityEngine.Debug.Log( v21 +" "+ v22 +" "+ v2);
* UnityEngine.Debug.Log( v31 +" "+ v32 +" "+ v3);
* UnityEngine.Debug.Log( v41 +" "+ v42 +" "+ v4);
* UnityEngine.Debug.Log( mn);
*
* UnityEngine.Debug.LogError(" ::END STATE");
*/
// Compute normal of the wedge face
TSVector.Subtract(ref v2, ref v1, out temp1);
TSVector.Subtract(ref v3, ref v1, out temp2);
TSVector.Cross(ref temp1, ref temp2, out normal);
// Beginer
// UnityEngine.Debug.Log("normal: " + normal);
// Can this happen??? Can it be handled more cleanly?
if (normal.IsNearlyZero())
{
return(true);
}
normal.Normalize();
//UnityEngine.Debug.Log("normal: " + normal);
// Compute distance from origin to wedge face
FP d = TSVector.Dot(ref normal, ref v1);
// If the origin is inside the wedge, we have a hit
if (d >= 0 && !hit)
{
// HIT!!!
hit = true;
}
// Find the support point in the direction of the wedge face
TSVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v41);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v42);
TSVector.Subtract(ref v42, ref v41, out v4);
TSVector.Subtract(ref v4, ref v3, out temp1);
FP delta = TSVector.Dot(ref temp1, ref normal);
penetration = TSVector.Dot(ref v4, ref normal);
// If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
if (delta <= CollideEpsilon || penetration <= FP.Zero || phase2 > MaximumIterations)
{
if (hit)
{
TSVector.Cross(ref v1, ref v2, out temp1);
FP b0 = TSVector.Dot(ref temp1, ref v3);
TSVector.Cross(ref v3, ref v2, out temp1);
FP b1 = TSVector.Dot(ref temp1, ref v0);
TSVector.Cross(ref v0, ref v1, out temp1);
FP b2 = TSVector.Dot(ref temp1, ref v3);
TSVector.Cross(ref v2, ref v1, out temp1);
FP b3 = TSVector.Dot(ref temp1, ref v0);
FP sum = b0 + b1 + b2 + b3;
if (sum <= 0)
{
b0 = 0;
TSVector.Cross(ref v2, ref v3, out temp1);
b1 = TSVector.Dot(ref temp1, ref normal);
TSVector.Cross(ref v3, ref v1, out temp1);
b2 = TSVector.Dot(ref temp1, ref normal);
TSVector.Cross(ref v1, ref v2, out temp1);
b3 = TSVector.Dot(ref temp1, ref normal);
sum = b1 + b2 + b3;
}
FP inv = FP.One / sum;
TSVector.Multiply(ref v01, b0, out point);
TSVector.Multiply(ref v11, b1, out temp1);
TSVector.Add(ref point, ref temp1, out point);
TSVector.Multiply(ref v21, b2, out temp1);
TSVector.Add(ref point, ref temp1, out point);
TSVector.Multiply(ref v31, b3, out temp1);
TSVector.Add(ref point, ref temp1, out point);
TSVector.Multiply(ref v02, b0, out temp2);
TSVector.Add(ref temp2, ref point, out point);
TSVector.Multiply(ref v12, b1, out temp1);
TSVector.Add(ref point, ref temp1, out point);
TSVector.Multiply(ref v22, b2, out temp1);
TSVector.Add(ref point, ref temp1, out point);
TSVector.Multiply(ref v32, b3, out temp1);
TSVector.Add(ref point, ref temp1, out point);
TSVector.Multiply(ref point, inv * FP.Half, out point);
}
// Compute the barycentric coordinates of the origin
return(hit);
}
//// Compute the tetrahedron dividing face (v4,v0,v1)
//JVector.Cross(ref v4, ref v1, out temp1);
//FP d1 = JVector.Dot(ref temp1, ref v0);
//// Compute the tetrahedron dividing face (v4,v0,v2)
//JVector.Cross(ref v4, ref v2, out temp1);
//FP d2 = JVector.Dot(ref temp1, ref v0);
// Compute the tetrahedron dividing face (v4,v0,v3)
//UnityEngine.Debug.LogError("v4:" + v4 + " v0:" + v0);
TSVector.Cross(ref v4, ref v0, out temp1);
//UnityEngine.Debug.LogError("temp1:"+ temp1);
//Ender
// UnityEngine.Debug.Log("normal: " + normal);
FP dot = TSVector.Dot(ref temp1, ref v1);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 temp1:" + temp1 + " v2:" + v2 );
dot = TSVector.Dot(ref temp1, ref v2);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 v1->v4");
// Inside d1 & inside d2 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
else
{
// UnityEngine.Debug.Log("dot < v3->v4");
// Inside d1 & outside d2 ==> eliminate v3
v3 = v4;
v31 = v41;
v32 = v42;
}
}
else
{
// UnityEngine.Debug.Log("dot < 0 temp1:" + temp1 + " v3:" + v3 );
dot = TSVector.Dot(ref temp1, ref v3);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 v2 => v4");
// Outside d1 & inside d3 ==> eliminate v2
v2 = v4;
v21 = v41;
v22 = v42;
}
else
{
// UnityEngine.Debug.Log("dot < 0 v1 => v4");
// Outside d1 & outside d3 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
}
}
}
}
private bool Collide(TSVector sphereCenter, FP r, ref TSVector[] vertices,
ref TSVector point, ref TSVector point1, ref TSVector point2, ref TSVector resultNormal, ref FP penetration)
{
TSVector c = sphereCenter;
TSVector delta = TSVector.one;
TSVector normal = TSVector.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]);
normal = TSVector.Normalize(normal);
TSVector p1ToCentre = c - vertices[0];
FP distanceFromPlane = TSVector.Dot(p1ToCentre, normal);
if (distanceFromPlane < FP.Zero)
{
//triangle facing the other way
distanceFromPlane *= -1;
normal *= -1;
}
FP contactMargin = FP.Zero; //TODO: PersistentManifold.ContactBreakingThreshold;
bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
bool isInsideShellPlane = distanceFromPlane < r;
FP deltaDotNormal = TSVector.Dot(delta, normal);
if (!isInsideShellPlane && deltaDotNormal >= FP.Zero)
{
return(false);
}
// Check for contact / intersection
bool hasContact = false;
TSVector contactPoint = TSVector.zero;
if (isInsideContactPlane)
{
if (FaceContains(ref c, ref vertices, ref normal))
{
// Inside the contact wedge - touches a point on the shell plane
hasContact = true;
contactPoint = c - normal * distanceFromPlane;
}
else
{
// Could be inside one of the contact capsules
FP contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
TSVector nearestOnEdge = TSVector.zero;
for (int i = 0; i < 3; i++)
{
TSVector pa, pb;
pa = vertices[i];
pb = vertices[(i + 1) % 3];
FP distanceSqr = SegmentSquareDistance(pa, pb, c, ref nearestOnEdge);
if (distanceSqr < contactCapsuleRadiusSqr)
{
// Yep, we're inside a capsule
contactCapsuleRadiusSqr = distanceSqr;
hasContact = true;
contactPoint = nearestOnEdge;
}
}
}
}
if (hasContact)
{
FP MaxOverlap = FP.Zero; // TODO:
TSVector contactToCentre = c - contactPoint;
FP distanceSqr = contactToCentre.sqrMagnitude;
if (distanceSqr < (r - MaxOverlap) * (r - MaxOverlap))
{
FP distance = TSMath.Sqrt(distanceSqr);
resultNormal = contactToCentre;
resultNormal = TSVector.Normalize(resultNormal);
point = contactPoint;
point1 = point;
resultNormal = TSVector.Negate(resultNormal);
point2 = sphereCenter + resultNormal * r;
penetration = r - distance;
return(true);
}
if (TSVector.Dot(delta, contactToCentre) >= FP.Zero)
{
return(false);
}
// Moving towards the contact point -> collision
point = point1 = point2 = contactPoint;
return(true);
}
return(false);
}
public bool UpdateClosestVectorAndPoints()
{
if (_needsUpdate)
{
_cachedBC.Reset();
_needsUpdate = false;
TSVector p, a, b, c, d;
switch (NumVertices)
{
case 0:
_cachedValidClosest = false;
break;
case 1:
_cachedPA = _simplexPointsP[0];
_cachedPB = _simplexPointsQ[0];
_cachedV = _cachedPA - _cachedPB;
_cachedBC.Reset();
_cachedBC.SetBarycentricCoordinates(FP.One, FP.Zero, FP.Zero, FP.Zero);
_cachedValidClosest = _cachedBC.IsValid;
break;
case 2:
//closest point origin from line segment
TSVector from = _simplexVectorW[0];
TSVector to = _simplexVectorW[1];
//TSVector nearest;
TSVector diff = TSVector.Negate(from);
TSVector v = to - from;
FP t = TSVector.Dot(v, diff);
if (t > FP.Zero)
{
FP dotVV = v.sqrMagnitude;
if (t < dotVV)
{
t /= dotVV;
diff -= t * v;
_cachedBC.usedVertices.UsedVertexA = true;
_cachedBC.usedVertices.UsedVertexB = true;
}
else
{
t = FP.One;
diff -= v;
//reduce to 1 point
_cachedBC.usedVertices.UsedVertexB = true;
}
}
else
{
t = FP.Zero;
//reduce to 1 point
_cachedBC.usedVertices.UsedVertexA = true;
}
_cachedBC.SetBarycentricCoordinates(FP.One - t, t, FP.Zero, FP.Zero);
//nearest = from + t * v;
_cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]);
_cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]);
_cachedV = _cachedPA - _cachedPB;
ReduceVertices(_cachedBC.usedVertices);
_cachedValidClosest = _cachedBC.IsValid;
break;
case 3:
//closest point origin from triangle
a = _simplexVectorW[0];
b = _simplexVectorW[1];
c = _simplexVectorW[2];
ClosestPtPointTriangle(a, b, c, ref _cachedBC);
_cachedPA = _simplexPointsP[0] * _cachedBC.barycentricCoords[0] +
_simplexPointsP[1] * _cachedBC.barycentricCoords[1] +
_simplexPointsP[2] * _cachedBC.barycentricCoords[2];
_cachedPB = _simplexPointsQ[0] * _cachedBC.barycentricCoords[0] +
_simplexPointsQ[1] * _cachedBC.barycentricCoords[1] +
_simplexPointsQ[2] * _cachedBC.barycentricCoords[2];
_cachedV = _cachedPA - _cachedPB;
ReduceVertices(_cachedBC.usedVertices);
_cachedValidClosest = _cachedBC.IsValid;
break;
case 4:
a = _simplexVectorW[0];
b = _simplexVectorW[1];
c = _simplexVectorW[2];
d = _simplexVectorW[3];
bool hasSeparation = ClosestPtPointTetrahedron(a, b, c, d, ref _cachedBC);
if (hasSeparation)
{
_cachedPA = _simplexPointsP[0] * _cachedBC.barycentricCoords[0] +
_simplexPointsP[1] * _cachedBC.barycentricCoords[1] +
_simplexPointsP[2] * _cachedBC.barycentricCoords[2] +
_simplexPointsP[3] * _cachedBC.barycentricCoords[3];
_cachedPB = _simplexPointsQ[0] * _cachedBC.barycentricCoords[0] +
_simplexPointsQ[1] * _cachedBC.barycentricCoords[1] +
_simplexPointsQ[2] * _cachedBC.barycentricCoords[2] +
_simplexPointsQ[3] * _cachedBC.barycentricCoords[3];
_cachedV = _cachedPA - _cachedPB;
ReduceVertices(_cachedBC.usedVertices);
}
else
{
if (_cachedBC.degenerate)
{
_cachedValidClosest = false;
}
else
{
_cachedValidClosest = true;
//degenerate case == false, penetration = true + zero
_cachedV.x = _cachedV.y = _cachedV.z = FP.Zero;
}
break; // !!!!!!!!!!!! proverit na vsakiy sluchai
}
_cachedValidClosest = _cachedBC.IsValid;
//closest point origin from tetrahedron
break;
default:
_cachedValidClosest = false;
break;
}
}
return(_cachedValidClosest);
}
public FP GetSphereDistance(ref BoxShape box, ref TSVector boxPosition, ref TSMatrix boxOrientation, ref TSVector sphereCenter, FP radius,
ref TSVector pointOnBox, ref TSVector pointOnSphere, ref TSVector normal)
{
TSVector boxHalfSize = box.halfSize;
TSVector prel;
// convert sphere center into box local space
TSVector.Subtract(ref sphereCenter, ref boxPosition, out prel);
TSMatrix invBoxOrientation;
TSMatrix.Inverse(ref boxOrientation, out invBoxOrientation);
TSVector.Transform(ref prel, ref invBoxOrientation, out prel);
pointOnBox = prel;
bool outSide = false;
//x
if (prel.x < -boxHalfSize.x)
{
outSide = true;
pointOnBox.x = -boxHalfSize.x;
}
else if (prel.x > boxHalfSize.x)
{
outSide = true;
pointOnBox.x = boxHalfSize.x;
}
//y
if (prel.y < -boxHalfSize.y)
{
outSide = true;
pointOnBox.y = -boxHalfSize.y;
}
else if (prel.y > boxHalfSize.y)
{
outSide = true;
pointOnBox.y = boxHalfSize.y;
}
//z
if (prel.z < -boxHalfSize.z)
{
outSide = true;
pointOnBox.z = -boxHalfSize.z;
}
else if (prel.z > boxHalfSize.z)
{
outSide = true;
pointOnBox.z = boxHalfSize.z;
}
if (outSide)
{
normal = prel - pointOnBox;
FP dist2 = normal.sqrMagnitude;
if (dist2 > radius * radius)
{
return(FP.One);
}
FP distance;
if (dist2 <= TSMath.Epsilon)
{
distance = -GetSpherePenetration(boxHalfSize, prel, ref pointOnBox, out normal);
}
else
{
distance = normal.magnitude;
normal /= distance;
}
// transform back in world space
TSVector.Transform(ref pointOnBox, ref boxOrientation, out pointOnBox);
TSVector.Add(ref pointOnBox, ref boxPosition, out pointOnBox);
pointOnSphere = prel - normal * radius;
TSVector.Transform(ref pointOnSphere, ref boxOrientation, out pointOnSphere);
TSVector.Add(ref pointOnSphere, ref boxPosition, out pointOnSphere);
normal = TSVector.Transform(normal, boxOrientation);
normal = TSVector.Negate(normal);
normal.Normalize();
return(distance - radius);
}
else
{
FP distance;
distance = -GetSpherePenetration(boxHalfSize, prel, ref pointOnBox, out normal);
normal = TSVector.Transform(normal, boxOrientation);
normal = TSVector.Negate(normal);
pointOnSphere = sphereCenter + normal * radius;
TSVector.Transform(ref pointOnBox, ref boxOrientation, out pointOnBox);
TSVector.Add(ref pointOnBox, ref boxPosition, out pointOnBox);
return(distance - radius);
}
}