/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
TSVector expandVector;
TSVector.Normalize(direction, out expandVector);
TSVector.Multiply(expandVector, sphericalExpansion, out expandVector);
int minIndex = 0;
FP min = TSVector.Dot(points[0], direction);
FP dot = TSVector.Dot(points[1], direction);
if (dot > min)
{
min = dot;
minIndex = 1;
}
dot = TSVector.Dot(points[2], direction);
if (dot > min)
{
min = dot;
minIndex = 2;
}
TSVector.Add(points[minIndex], expandVector, out result);
}
/// <summary>
/// Multiplies a vector by a scale factor.
/// </summary>
/// <param name="value2">The vector to scale.</param>
/// <param name="value1">The scale factor.</param>
/// <returns>Returns the scaled vector.</returns>
#region public static JVector operator *(FP value1, JVector value2)
public static TSVector operator *(FP value1, TSVector value2)
{
TSVector result;
TSVector.Multiply(ref value2, value1, out result);
return(result);
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
result = direction;
result.Normalize();
TSVector.Multiply(result, radius, out 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);
}
private void IntegrateForces()
{
for (int index = 0, length = rigidBodies.Count; index < length; index++)
{
RigidBody body = rigidBodies[index];
if (!body.isStatic && body.IsActive)
{
TSVector temp;
TSVector.Multiply(ref body.force, body.inverseMass * timestep, out temp);
TSVector.Add(ref temp, ref body.linearVelocity, out body.linearVelocity);
if (!(body.isParticle))
{
TSVector.Multiply(ref body.torque, timestep, out temp);
TSVector.Transform(ref temp, ref body.invInertiaWorld, out temp);
TSVector.Add(ref temp, ref body.angularVelocity, out body.angularVelocity);
}
if (body.affectedByGravity)
{
TSVector.Multiply(ref gravity, timestep, out temp);
TSVector.Add(ref body.linearVelocity, ref temp, out body.linearVelocity);
}
}
body.force.MakeZero();
body.torque.MakeZero();
}
}
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 void SupportCenter(out TSVector center)
{
center = owner.points[indices.I0].position;
TSVector.Add(center, owner.points[indices.I1].position, out center);
TSVector.Add(center, owner.points[indices.I2].position, out center);
TSVector.Multiply(center, FP.One / (3 * FP.One), out center);
}
/// <summary>
/// Multiply a vector with a factor.
/// </summary>
/// <param name="value1">The vector to multiply.</param>
/// <param name="scaleFactor">The scale factor.</param>
/// <returns>Returns the multiplied vector.</returns>
#region public static JVector Multiply(JVector value1, FP scaleFactor)
public static TSVector Multiply(TSVector value1, FP scaleFactor)
{
TSVector result;
TSVector.Multiply(ref value1, scaleFactor, out result);
return(result);
}
private void IntegrateCallback(object obj)
{
RigidBody body = obj as RigidBody;
TSVector temp;
TSVector.Multiply(ref body.linearVelocity, timestep, out temp);
TSVector.Add(ref temp, ref body.position, out body.position);
if (!(body.isParticle))
{
//exponential map
TSVector axis;
FP angle = body.angularVelocity.magnitude;
FP halfTimeStep = FP.Half * timestep;
FP halfTimeStepAngle = halfTimeStep * angle;
if (angle < FP.EN3)
{
// use Taylor's expansions of sync function
// axis = body.angularVelocity * (FP.Half * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
//TSVector.Multiply(ref body.angularVelocity, (halfTimeStep - (timestep * timestep * timestep) * (2082 * FP.EN6) * angle * angle), out axis);
TSVector.Multiply(ref body.angularVelocity, halfTimeStep, out axis);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
TSVector.Multiply(ref body.angularVelocity, (FP.Sin(halfTimeStepAngle) / angle), out axis);
}
TSQuaternion dorn = new TSQuaternion(axis.x, axis.y, axis.z, FP.Cos(halfTimeStepAngle));
TSQuaternion ornA;
TSQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
TSQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize();
TSMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
body.linearVelocity /= (1 + timestep * body.linearDrag);
body.angularVelocity /= (1 + timestep * body.angularDrag);
/*if ((body.Damping & RigidBody.DampingType.Linear) != 0)
* TSVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
*
* if ((body.Damping & RigidBody.DampingType.Angular) != 0)
* TSVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);*/
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
{
body.SweptExpandBoundingBox(timestep);
}
}
private void Integrate()
{
for (int index = 0, length = rigidBodies.Count; index < length; index++)
{
var body = rigidBodies[index];
if (body.isStatic || !body.IsActive)
{
continue;
}
body.position += body.linearVelocity * timestep;
if (!(body.isParticle))
{
//exponential map
TSVector axis;
FP angle = body.angularVelocity.magnitude;
if (angle < FP.EN3)
{
// use Taylor's expansions of sync function
// axis = body.angularVelocity * (FP.Half * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
TSVector.Multiply(body.angularVelocity, (FP.Half * timestep - (timestep * timestep * timestep) * (2082 * FP.EN6) * angle * angle), out axis);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
TSVector.Multiply(body.angularVelocity, (FP.Sin(FP.Half * angle * timestep) / angle), out axis);
}
TSQuaternion dorn = new TSQuaternion(axis.x, axis.y, axis.z, FP.Cos(angle * timestep * FP.Half));
TSQuaternion ornA; TSQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
TSQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize(); TSMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
body.linearVelocity *= 1 / (1 + timestep * body.linearDrag);
body.angularVelocity *= 1 / (1 + timestep * body.angularDrag);
/*if ((body.Damping & RigidBody.DampingType.Linear) != 0)
* TSVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
*
* if ((body.Damping & RigidBody.DampingType.Angular) != 0)
* TSVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);*/
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
{
body.SweptExpandBoundingBox(timestep);
}
}
}
/// <summary>
/// Applies an impulse on the center of the body. Changing
/// linear velocity.
/// </summary>
/// <param name="impulse">Impulse direction and magnitude.</param>
public void ApplyImpulse(TSVector impulse)
{
if (this.isStatic)
{
return;
}
TSVector temp;
TSVector.Multiply(ref impulse, inverseMass, out temp);
TSVector.Add(ref linearVelocity, ref temp, out linearVelocity);
}
/// <summary>
/// Applies an impulse on the center of the body. Changing
/// linear velocity.
/// </summary>
/// <param name="impulse">Impulse direction and magnitude.</param>
public void ApplyImpulse(TSVector impulse)
{
if (this.isStatic || impulse.IsZero())
{
return;
}
//UnityEngine.Debug.Log($"ApplyImpulse {impulse}");
TSVector temp;
TSVector.Multiply(impulse, inverseMass, out temp);
TSVector.Add(linearVelocity, temp, out linearVelocity);
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
if (skipConstraint)
{
return;
}
FP jv = TSVector.Dot(ref body1.linearVelocity, ref jacobian[0]);
jv += TSVector.Dot(ref body2.linearVelocity, ref jacobian[1]);
FP softnessScalar = accumulatedImpulse * softnessOverDt;
FP lambda = -effectiveMass * (jv + bias + softnessScalar);
if (behavior == DistanceBehavior.LimitMinimumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = TSMath.Max(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else if (behavior == DistanceBehavior.LimitMaximumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = TSMath.Min(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else
{
accumulatedImpulse += lambda;
}
TSVector temp;
CBFrame.Utils.Logger.Debug("line195 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body1.isStatic)
{
TSVector.Multiply(ref jacobian[0], lambda * body1.inverseMass, out temp);
TSVector.Add(ref temp, ref body1.linearVelocity, out body1.linearVelocity);
}
CBFrame.Utils.Logger.Debug("line201 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body2.isStatic)
{
TSVector.Multiply(ref jacobian[1], lambda * body2.inverseMass, out temp);
TSVector.Add(ref temp, ref body2.linearVelocity, out body2.linearVelocity);
}
CBFrame.Utils.Logger.Debug("line206 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
}
/// <summary>
/// Applies an impulse on the specific position. Changing linear
/// and angular velocity.
/// </summary>
/// <param name="impulse">Impulse direction and magnitude.</param>
/// <param name="relativePosition">The position where the impulse gets applied
/// in Body coordinate frame.</param>
public void ApplyImpulse(TSVector impulse, TSVector relativePosition)
{
if (this.isStatic || impulse.IsZero())
{
return;
}
TSVector temp;
TSVector.Multiply(impulse, inverseMass, out temp);
TSVector.Add(linearVelocity, temp, out linearVelocity);
TSVector.Cross(relativePosition, impulse, out temp);
TSVector.Transform(temp, invInertiaWorld, out temp);
TSVector.Add(angularVelocity, temp, out angularVelocity);
}
/// <summary>
/// Applies an impulse on the specific position. Changing linear
/// and angular velocity.
/// </summary>
/// <param name="impulse">Impulse direction and magnitude.</param>
/// <param name="relativePosition">The position where the impulse gets applied
/// in Body coordinate frame.</param>
public void ApplyImpulse(TSVector impulse, TSVector relativePosition)
{
if (this.isStatic)
{
return;
}
TSVector temp;
TSVector.Multiply(ref impulse, inverseMass, out temp);
TSVector.Add(ref linearVelocity, ref temp, out linearVelocity);
TSVector.Cross(ref relativePosition, ref impulse, out temp);
TSVector.Transform(ref temp, ref invInertiaWorld, out temp);
TSVector.Add(ref angularVelocity, ref temp, out angularVelocity);
}
/// <summary>
/// Create a bounding box appropriate for a child, based on a parents AABox
/// </summary>
/// <param name="aabb"></param>
/// <param name="child"></param>
/// <param name="result"></param>
#region private void CreateAABox(ref JBBox aabb, EChild child,out JBBox result)
private void CreateAABox(ref TSBBox aabb, EChild child, out TSBBox result)
{
TSVector dims;
TSVector.Subtract(ref aabb.max, ref aabb.min, out dims);
TSVector.Multiply(ref dims, FP.Half, out dims);
TSVector offset = TSVector.zero;
switch (child)
{
case EChild.PPP: offset = new TSVector(1, 1, 1); break;
case EChild.PPM: offset = new TSVector(1, 1, 0); break;
case EChild.PMP: offset = new TSVector(1, 0, 1); break;
case EChild.PMM: offset = new TSVector(1, 0, 0); break;
case EChild.MPP: offset = new TSVector(0, 1, 1); break;
case EChild.MPM: offset = new TSVector(0, 1, 0); break;
case EChild.MMP: offset = new TSVector(0, 0, 1); break;
case EChild.MMM: offset = new TSVector(0, 0, 0); break;
default:
System.Diagnostics.Debug.WriteLine("Octree.CreateAABox got impossible child");
break;
}
result = new TSBBox();
result.min = new TSVector(offset.x * dims.x, offset.y * dims.y, offset.z * dims.z);
TSVector.Add(ref result.min, ref aabb.min, out result.min);
TSVector.Add(ref result.min, ref dims, out result.max);
// expand it just a tiny bit just to be safe!
FP extra = FP.EN5;
TSVector temp; TSVector.Multiply(ref dims, extra, out temp);
TSVector.Subtract(ref result.min, ref temp, out result.min);
TSVector.Add(ref result.max, ref temp, out result.max);
}
private void IntegrateForces()
{
for (int index = 0, length = rigidBodies.Count; index < length; index++)
{
RigidBody body = rigidBodies[index];
if (!body.isStatic && body.IsActive)
{
CBFrame.Utils.Logger.Debug("line787 linearVelocity:" + body.linearVelocity);
TSVector temp;
TSVector.Multiply(ref body.force, body.inverseMass * timestep, out temp);
TSVector.Add(ref temp, ref body.linearVelocity, out body.linearVelocity);
CBFrame.Utils.Logger.Debug("line793 linearVelocity:" + body.linearVelocity);
if (!(body.isParticle))
{
TSVector.Multiply(ref body.torque, timestep, out temp);
TSVector.Transform(ref temp, ref body.invInertiaWorld, out temp);
TSVector.Add(ref temp, ref body.angularVelocity, out body.angularVelocity);
}
CBFrame.Utils.Logger.Debug("line801 linearVelocity:" + body.linearVelocity);
if (body.affectedByGravity)
{
if (body.linearVelocity.y <= -0.0000741470 && body.linearVelocity.y >= -0.0000741475)
{
Debug.Log("body.linearVelocity:" + body.linearVelocity);
}
TSVector.Multiply(ref gravity, timestep, out temp);
CBFrame.Utils.Logger.Debug("line809 gravity:" + gravity + ",timestep:" + timestep +
",temp:" + temp + ",body.linearVelocity:" + body.linearVelocity);
TSVector.Add(ref body.linearVelocity, ref temp, out body.linearVelocity);
}
if (body.linearVelocity.y <= -0.2000741000 && body.linearVelocity.y >= -0.2000742000)
{
Debug.Log("body.linearVelocity:" + body.linearVelocity);
}
CBFrame.Utils.Logger.Debug("line814 linearVelocity:" + body.linearVelocity);
}
body.force.MakeZero();
body.torque.MakeZero();
}
}
/// <summary>
/// Applies an impulse on the center of the body. Changing
/// linear velocity.
/// </summary>
/// <param name="impulse">Impulse direction and magnitude.</param>
public void ApplyImpulse(TSVector impulse)
{
if (this.isStatic)
{
return;
}
CBFrame.Utils.Logger.Debug("line314 linearVelocity:" + linearVelocity);
TSVector temp;
TSVector.Multiply(ref impulse, inverseMass, out temp);
TSVector.Add(ref linearVelocity, ref temp, out linearVelocity);
CBFrame.Utils.Logger.Debug("line320 linearVelocity:" + linearVelocity);
if (linearVelocity.y._serializedValue == 17809140)
{
UnityEngine.Debug.Log("linearVelocity:" + linearVelocity);
}
}
/// <summary>
/// Gets the closest point on the segment to the given point.
/// </summary>
/// <param name="p">The point with which to calculate the nearest segment point.</param>
/// <param name="scalar">Returns a value between 0 and 1 indicating the location on the segment nearest the point.</param>
/// <param name="output">Returns the closest point on the segment.</param>
public void ClosestPointTo(ref TSVector p, out FP scalar, out TSVector output)
{
TSVector u, v;
TSVector.Subtract(ref p, ref P1, out v);
TSVector.Subtract(ref P2, ref P1, out u);
scalar = TSVector.Dot(ref u, ref v);
scalar /= u.sqrMagnitude;
if (scalar <= FP.Zero)
{
output = P1;
}
else if (scalar >= FP.One)
{
output = P2;
}
else
{
TSVector.Multiply(ref u, scalar, out output);
TSVector.Add(ref P1, ref output, out output);
}
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
TSVector expandVector;
TSVector.Normalize(ref direction, out expandVector);
TSVector.Multiply(ref expandVector, sphericalExpansion, out expandVector);
int minIndex = 0;
FP min = TSVector.Dot(ref points[0], ref direction);
for (int i = 1, length = points.Length; i < length; i++)
{
FP dot = TSVector.Dot(ref points[i], ref direction);
if (dot > min)
{
min = dot;
minIndex = i;
}
}
TSVector.Add(ref points[minIndex], ref expandVector, out result);
}
/// <summary>
/// Sets the current shape. First <see cref="Prepare"/> has to be called.
/// After SetCurrentShape the shape immitates another shape.
/// </summary>
/// <param name="index"></param>
public override void SetCurrentShape(int index)
{
bool leftTriangle = false;
if (index >= numX * numZ)
{
leftTriangle = true;
index -= numX * numZ;
}
int quadIndexX = index % numX;
int quadIndexZ = index / numX;
// each quad has two triangles, called 'leftTriangle' and !'leftTriangle'
if (leftTriangle)
{
points[0].Set((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[1].Set((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[2].Set((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
}
else
{
points[0].Set((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[1].Set((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
points[2].Set((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
}
TSVector sum = points[0];
TSVector.Add(sum, points[1], out sum);
TSVector.Add(sum, points[2], out sum);
TSVector.Multiply(sum, FP.One / (3 * FP.One), out sum);
geomCen = sum;
TSVector.Subtract(points[1], points[0], out sum);
TSVector.Subtract(points[2], points[0], out normal);
TSVector.Cross(sum, normal, out normal);
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
FP r = FP.Sqrt(direction.x * direction.x + direction.z * direction.z);
if (FP.Abs(direction.y) > FP.Zero)
{
TSVector dir; TSVector.Normalize(direction, out dir);
TSVector.Multiply(dir, radius, out result);
result.y += FP.Sign(direction.y) * FP.Half * length;
}
else if (r > FP.Zero)
{
result.x = direction.x / r * radius;
result.y = FP.Zero;
result.z = direction.z / r * radius;
}
else
{
result.x = FP.Zero;
result.y = FP.Zero;
result.z = FP.Zero;
}
}
private void IntegrateForces()
{
for (int index = 0, length = rigidBodies.Count; index < length; index++)
{
RigidBody body = rigidBodies[index];
if (!body.isStatic && body.IsActive)
{
TSVector temp;
TSVector.Multiply(ref body.force, body.inverseMass * timestep, out temp);
TSVector.Add(ref temp, ref body.linearVelocity, out body.linearVelocity);
if (!(body.isParticle))
{
TSVector.Multiply(ref body.torque, timestep, out temp);
TSVector.Transform(ref temp, ref body.invInertiaWorld, out temp);
TSVector.Add(ref temp, ref body.angularVelocity, out body.angularVelocity);
//clamp angular velocity to 0.5pi .
FP maxAngVel = TSMath.PiOver2;
FP angvel = body.angularVelocity.magnitude;
if (angvel * timestep > maxAngVel)
{
body.angularVelocity *= (maxAngVel / timestep) / angvel;
}
}
if (body.affectedByGravity)
{
TSVector.Multiply(ref gravity, timestep, out temp);
TSVector.Add(ref body.linearVelocity, ref temp, out body.linearVelocity);
}
}
body.force.MakeZero();
body.torque.MakeZero();
}
}
/// <summary>
/// Sets the current shape. First <see cref="Prepare"/> has to be called.
/// After SetCurrentShape the shape immitates another shape.
/// </summary>
/// <param name="index"></param>
public override void SetCurrentShape(int index)
{
vecs[0] = octree.GetVertex(octree.tris[potentialTriangles[index]].I0);
vecs[1] = octree.GetVertex(octree.tris[potentialTriangles[index]].I1);
vecs[2] = octree.GetVertex(octree.tris[potentialTriangles[index]].I2);
TSVector sum = vecs[0];
TSVector.Add(ref sum, ref vecs[1], out sum);
TSVector.Add(ref sum, ref vecs[2], out sum);
TSVector.Multiply(ref sum, FP.One / (3 * FP.One), out sum);
geomCen = sum;
TSVector.Subtract(ref vecs[1], ref vecs[0], out sum);
TSVector.Subtract(ref vecs[2], ref vecs[0], out normal);
TSVector.Cross(ref sum, ref normal, out normal);
normal.Normalize();
if (flipNormal)
{
normal.Negate();
}
}
// public static bool TimeOfImpact(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
//ref JMatrix orientation2, ref JVector position1, ref JVector position2, ref JVector sweptA, ref JVector sweptB,
//out JVector p1, out JVector p2, out JVector normal)
// {
// VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
// simplexSolver.Reset();
// FP lambda = FP.Zero;
// p1 = p2 = JVector.Zero;
// JVector x1 = position1;
// JVector x2 = position2;
// JVector r = sweptA - sweptB;
// JVector w, v;
// JVector supVertexA;
// JVector rn = JVector.Negate(r);
// SupportMapTransformed(support1, ref orientation1, ref x1, ref rn, out supVertexA);
// JVector supVertexB;
// SupportMapTransformed(support2, ref orientation2, ref x2, ref r, out supVertexB);
// v = supVertexA - supVertexB;
// bool hasResult = false;
// normal = JVector.Zero;
// FP lastLambda = lambda;
// int maxIter = MaxIterations;
// FP distSq = v.LengthSquared();
// FP epsilon = FP.EN5;
// FP VdotR;
// while ((distSq > epsilon) && (maxIter-- != 0))
// {
// JVector vn = JVector.Negate(v);
// SupportMapTransformed(support1, ref orientation1, ref x1, ref vn, out supVertexA);
// SupportMapTransformed(support2, ref orientation2, ref x2, ref v, out supVertexB);
// w = supVertexA - supVertexB;
// FP VdotW = JVector.Dot(ref v, ref w);
// if (VdotW > FP.Zero)
// {
// VdotR = JVector.Dot(ref v, ref r);
// if (VdotR >= -JMath.Epsilon)
// {
// simplexSolverPool.GiveBack(simplexSolver);
// return false;
// }
// else
// {
// lambda = lambda - VdotW / VdotR;
// x1 = position1 + lambda * sweptA;
// x2 = position2 + lambda * sweptB;
// w = supVertexA - supVertexB;
// normal = v;
// hasResult = true;
// }
// }
// if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, supVertexA, supVertexB);
// if (simplexSolver.Closest(out v))
// {
// distSq = v.LengthSquared();
// normal = v;
// hasResult = true;
// }
// else distSq = FP.Zero;
// }
// simplexSolver.ComputePoints(out p1, out p2);
// if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
// normal.Normalize();
// p1 = p1 - lambda * sweptA;
// p2 = p2 - lambda * sweptB;
// simplexSolverPool.GiveBack(simplexSolver);
// return true;
// }
#endregion
// see: btSubSimplexConvexCast.cpp
/// <summary>
/// Checks if a ray definied through it's origin and direction collides
/// with a shape.
/// </summary>
/// <param name="support">The supportmap implementation representing the shape.</param>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="invOrientation">The inverse orientation of the shape.</param>
/// <param name="position">The position of the shape.</param>
/// <param name="origin">The origin of the ray.</param>
/// <param name="direction">The direction of the ray.</param>
/// <param name="fraction">The fraction which gives information where at the
/// ray the collision occured. The hitPoint is calculated by: origin+friction*direction.</param>
/// <param name="normal">The normal from the ray collision.</param>
/// <returns>Returns true if the ray hit the shape, false otherwise.</returns>
public static bool Raycast(ISupportMappable support, ref TSMatrix orientation, ref TSMatrix invOrientation,
ref TSVector position, ref TSVector origin, ref TSVector direction, out FP fraction, out TSVector normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
normal = TSVector.zero;
fraction = FP.MaxValue;
FP lambda = FP.Zero;
TSVector r = direction;
TSVector x = origin;
TSVector w, p, v;
TSVector arbitraryPoint;
SupportMapTransformed(support, ref orientation, ref position, ref r, out arbitraryPoint);
TSVector.Subtract(ref x, ref arbitraryPoint, out v);
int maxIter = MaxIterations;
FP distSq = v.sqrMagnitude;
FP epsilon = FP.EN6;
FP VdotR;
while ((distSq > epsilon) && (maxIter-- != 0))
{
SupportMapTransformed(support, ref orientation, ref position, ref v, out p);
TSVector.Subtract(ref x, ref p, out w);
FP VdotW = TSVector.Dot(ref v, ref w);
if (VdotW > FP.Zero)
{
VdotR = TSVector.Dot(ref v, ref r);
if (VdotR >= -TSMath.Epsilon)
{
simplexSolverPool.GiveBack(simplexSolver);
return(false);
}
else
{
lambda = lambda - VdotW / VdotR;
TSVector.Multiply(ref r, lambda, out x);
TSVector.Add(ref origin, ref x, out x);
TSVector.Subtract(ref x, ref p, out w);
normal = v;
}
}
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, x, p);
}
if (simplexSolver.Closest(out v))
{
distSq = v.sqrMagnitude;
}
else
{
distSq = FP.Zero;
}
}
#region Retrieving hitPoint
// Giving back the fraction like this *should* work
// but is inaccurate against large objects:
// fraction = lambda;
TSVector p1, p2;
simplexSolver.ComputePoints(out p1, out p2);
p2 = p2 - origin;
fraction = p2.magnitude / direction.magnitude;
#endregion
if (normal.sqrMagnitude > TSMath.Epsilon * TSMath.Epsilon)
{
normal.Normalize();
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
/// <summary>
/// Gets the closest two points on two line segments.
/// </summary>
/// <remarks>
/// If the line segments are parallel and overlap in their common direction, then the midpoint of the overlapped portion of line segments
/// is returned.
/// </remarks>
/// <param name="sa">The first line segment.</param>
/// <param name="sb">The second line segment.</param>
/// <param name="scalarA">Returns a value between 0 and 1 indicating the position of the closest point on the first segment.</param>
/// <param name="pa">Returns the closest point on the first segment.</param>
/// <param name="scalarB">Returns a value between 0 and 1 indicating the position of the closest point on the second segment.</param>
/// <param name="pb">Returns the closest point on the second segment.</param>
public static void ClosestPoints(ref SegmentShape sa, ref SegmentShape sb,
out FP scalarA, out TSVector pa, out FP scalarB, out TSVector pb)
{
TSVector d1, d2, r;
TSVector.Subtract(ref sa.P2, ref sa.P1, out d1);
TSVector.Subtract(ref sb.P2, ref sb.P1, out d2);
TSVector.Subtract(ref sa.P1, ref sb.P1, out r);
FP a, e, f;
a = TSVector.Dot(ref d1, ref d1);
e = TSVector.Dot(ref d2, ref d2);
f = TSVector.Dot(ref d2, ref r);
if (a < TSMath.Epsilon && e < TSMath.Epsilon)
{
// segment a and b are both points
scalarA = scalarB = FP.Zero;
pa = sa.P1;
pb = sb.P1;
return;
}
if (a < TSMath.Epsilon)
{
// segment a is a point
scalarA = FP.Zero;
scalarB = TSMath.Clamp(f / e, FP.Zero, FP.One);
}
else
{
FP c = TSVector.Dot(ref d1, ref r);
if (e < TSMath.Epsilon)
{
// segment b is a point
scalarB = FP.Zero;
scalarA = TSMath.Clamp(-c / a, FP.Zero, FP.One);
}
else
{
FP b = TSVector.Dot(ref d1, ref d2);
FP denom = a * e - b * b;
if (denom < TSMath.Epsilon)
{
// segments are parallel
FP a1, a2, b1, b2;
a1 = TSVector.Dot(ref d2, ref sa.P1);
a2 = TSVector.Dot(ref d2, ref sa.P2);
b1 = TSVector.Dot(ref d2, ref sb.P1);
b2 = TSVector.Dot(ref d2, ref sb.P2);
if (a1 <= b1 && a2 <= b1)
{
// segment A is completely "before" segment B
scalarA = a2 > a1 ? FP.One : FP.Zero;
scalarB = FP.Zero;
}
else if (a1 >= b2 && a2 >= b2)
{
// segment B is completely "before" segment A
scalarA = a2 > a1 ? FP.Zero : FP.One;
scalarB = FP.One;
}
else
{
// segments A and B overlap, use midpoint of shared length
if (a1 > a2)
{
f = a1; a1 = a2; a2 = f;
}
f = (TSMath.Min(a2, b2) + TSMath.Max(a1, b1)) / 2;
scalarB = (f - b1) / e;
TSVector.Multiply(ref d2, scalarB, out pb);
TSVector.Add(ref sb.P1, ref pb, out pb);
sa.ClosestPointTo(ref pb, out scalarA, out pa);
return;
}
}
else
{
// general case
scalarA = TSMath.Clamp((b * f - c * e) / denom, FP.Zero, FP.One);
scalarB = (b * scalarA + f) / e;
if (scalarB < FP.Zero)
{
scalarB = FP.Zero;
scalarA = TSMath.Clamp(-c / a, FP.Zero, FP.One);
}
else if (scalarB > FP.One)
{
scalarB = FP.One;
scalarA = TSMath.Clamp((b - c) / a, FP.Zero, FP.One);
}
}
}
}
TSVector.Multiply(ref d1, scalarA, out d1);
TSVector.Multiply(ref d2, scalarB, out d2);
TSVector.Add(ref sa.P1, ref d1, out pa);
TSVector.Add(ref sb.P1, ref d2, out pb);
}
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);
}
/// <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;
}
}
}
}
}
