public void TapNearLine(TSVector tapLocation)
{
Debug.LogWarning("TAPNEARLINE");
TSRigidBody lineRB = GetClosestLine(tapLocation);
if (lineRB.angularVelocity.magnitude > 0) // exit this method if line is moving, can't hit lines while they move
{
return;
}
FP distanceToLine = GetDistanceToLineFromPosition(lineRB, tapLocation);
FP direction = TSVector.Cross(tapLocation.normalized, lineRB.tsTransform.forward).normalized.y;
FP tapEffectRadius = PlayerConfig.Instance.TapEffectRadius;
FP minTapDistance = PlayerConfig.Instance.MinTapDistance;
FP effect = FP.One;
if (distanceToLine < minTapDistance) // apply penalty, tap was too close to the line
{
ApplyExplosiveForceOnLine(lineRB, -1 * direction * PlayerConfig.Instance.PenaltyEffectForce);
}
else if (tapEffectRadius >= distanceToLine) // within range, but not on line. Apply normal force
{
effect = 1 - (distanceToLine - minTapDistance) / tapEffectRadius;
ApplyExplosiveForceOnLine(lineRB, direction * PlayerConfig.Instance.TapEffectForce * effect);
}
}
private bool PointInTriangle(ref TSVector[] vertices, ref TSVector normal, ref TSVector p)
{
TSVector p1 = vertices[0];
TSVector p2 = vertices[1];
TSVector p3 = vertices[2];
TSVector edge1 = p2 - p1;
TSVector edge2 = p3 - p2;
TSVector edge3 = p1 - p3;
TSVector p1ToP = p - p1;
TSVector p2ToP = p - p2;
TSVector p3ToP = p - p3;
TSVector edge1Normal = TSVector.Cross(edge1, normal);
TSVector edge2Normal = TSVector.Cross(edge2, normal);
TSVector edge3Normal = TSVector.Cross(edge3, normal);
FP r1, r2, r3;
r1 = TSVector.Dot(edge1Normal, p1ToP);
r2 = TSVector.Dot(edge2Normal, p2ToP);
r3 = TSVector.Dot(edge3Normal, p3ToP);
if ((r1 > FP.Zero && r2 > FP.Zero && r3 > FP.Zero) ||
(r1 <= FP.Zero && r2 <= FP.Zero && r3 <= FP.Zero))
{
return(true);
}
return(false);
}
/// <summary>
/// Adds a force to the center of the body. The force gets applied
/// the next time <see cref="World.Step"/> is called. The 'impact'
/// of the force depends on the time it is applied to a body - so
/// the timestep influences the energy added to the body.
/// </summary>
/// <param name="force">The force to add next <see cref="World.Step"/>.</param>
/// <param name="pos">The position where the force is applied.</param>
public void AddForce(TSVector force, TSVector pos)
{
TSVector.Add(ref this.force, ref force, out this.force);
TSVector.Subtract(ref pos, ref this.position, out pos);
TSVector.Cross(ref pos, ref force, out pos);
TSVector.Add(ref pos, ref this.torque, out this.torque);
}
/// <summary>
/// Calculates relative velocity of body contact points on the bodies.
/// </summary>
public TSVector CalculateRelativeVelocity()
{
var v2 = TSVector.Cross(body2.angularVelocity, relativePos2) + body2.linearVelocity;
var v1 = TSVector.Cross(body1.angularVelocity, relativePos1) + body1.linearVelocity;
return(v2 - v1);
}
/// <summary>
/// Adds a force to the center of the body. The force gets applied
/// the next time <see cref="World.Step"/> is called. The 'impact'
/// of the force depends on the time it is applied to a body - so
/// the timestep influences the energy added to the body.
/// </summary>
/// <param name="force">The force to add next <see cref="World.Step"/>.</param>
/// <param name="pos">The position where the force is applied.</param>
public void AddForce(TSVector force, TSVector pos)
{
TSVector.Add(this.force, force, out this.force);
TSVector.Subtract(pos, this.position, out pos);
TSVector.Cross(pos, force, out pos);
TSVector.Add(pos, this.torque, out this.torque);
}
/// <summary>
/// The cross product of two vectors.
/// </summary>
/// <param name="vector1">The first vector.</param>
/// <param name="vector2">The second vector.</param>
/// <returns>The cross product of both vectors.</returns>
#region public static JVector Cross(JVector vector1, JVector vector2)
public static TSVector Cross(TSVector vector1, TSVector vector2)
{
TSVector result;
TSVector.Cross(ref vector1, ref vector2, out result);
return(result);
}
public void GetNormal(out TSVector normal)
{
TSVector sum;
TSVector.Subtract(owner.points[indices.I1].position, owner.points[indices.I0].position, out sum);
TSVector.Subtract(owner.points[indices.I2].position, owner.points[indices.I0].position, out normal);
TSVector.Cross(sum, normal, out normal);
}
public static TSQuaternion FromToRotation(TSVector fromVector, TSVector toVector)
{
TSVector w = TSVector.Cross(fromVector, toVector);
TSQuaternion q = new TSQuaternion(w.x, w.y, w.z, TSVector.Dot(fromVector, toVector));
q.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude);
q.Normalize();
return(q);
}
// sort cached points so most isolated points come first
//排序缓存点，因此大多数孤立点最先出现。
private int SortCachedPoints(ref TSVector realRelPos1, FP pen)
{
//calculate 4 possible cases areas, and take biggest area
//also need to keep 'deepest'
//计算4个可能的cases区域，并采取最大的区域也需要保持“最深”
int maxPenetrationIndex = -1;//最穿透的序号
FP maxPenetration = pen;
for (int i = 0; i < 4; i++)
{
if (contactList[i].penetration > maxPenetration)
{
maxPenetrationIndex = i;
maxPenetration = contactList[i].penetration;
}
}
FP res0 = 0, res1 = 0, res2 = 0, res3 = 0;
if (maxPenetrationIndex != 0)
{
TSVector a0; TSVector.Subtract(ref realRelPos1, ref contactList[1].relativePos1, out a0);
TSVector b0; TSVector.Subtract(ref contactList[3].relativePos1, ref contactList[2].relativePos1, out b0);
TSVector cross; TSVector.Cross(ref a0, ref b0, out cross);
res0 = cross.sqrMagnitude;
}
if (maxPenetrationIndex != 1)
{
TSVector a0; TSVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
TSVector b0; TSVector.Subtract(ref contactList[3].relativePos1, ref contactList[2].relativePos1, out b0);
TSVector cross; TSVector.Cross(ref a0, ref b0, out cross);
res1 = cross.sqrMagnitude;
}
if (maxPenetrationIndex != 2)
{
TSVector a0; TSVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
TSVector b0; TSVector.Subtract(ref contactList[3].relativePos1, ref contactList[1].relativePos1, out b0);
TSVector cross; TSVector.Cross(ref a0, ref b0, out cross);
res2 = cross.sqrMagnitude;
}
if (maxPenetrationIndex != 3)
{
TSVector a0; TSVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
TSVector b0; TSVector.Subtract(ref contactList[2].relativePos1, ref contactList[1].relativePos1, out b0);
TSVector cross; TSVector.Cross(ref a0, ref b0, out cross);
res3 = cross.sqrMagnitude;
}
int biggestarea = MaxAxis(res0, res1, res2, res3);
return(biggestarea);
}
public static FP CalcK(RigidBody obj, TSVector relativePos, TSVector _normal)
{
if (!obj.IsStatic)
{
var v = TSVector.Cross(relativePos, _normal);
v = obj.invInertiaWorld.TransposedMultiply(v);
v = TSVector.Cross(v, relativePos);
return(obj.inverseMass + TSVector.Dot(v, _normal));
}
return(0);
}
/// <summary>
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
/// </summary>
/// <param name="b">The b.</param>
/// <returns></returns>
public TSVector Solve33(TSVector b)
{
FP det = TSVector.Dot(ex, TSVector.Cross(ey, ez));
if (det != 0.0f)
{
det = 1.0f / det;
}
return(new TSVector(det * TSVector.Dot(b, TSVector.Cross(ey, ez)), det * TSVector.Dot(ex, TSVector.Cross(b, ez)), det * TSVector.Dot(ex, TSVector.Cross(ey, b))));
}
//https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
private bool RayIntersectsTriangle(ISupportMappable support, ref TSMatrix orientation, ref TSMatrix invOrientation,
ref TSVector position, ref TSVector origin, ref TSVector direction, out FP fraction, out TSVector normal)
{
FP EPSILON = FP.EN8;
fraction = FP.Zero;
normal = TSVector.zero;
TriangleMeshShape inTriangle = support as TriangleMeshShape;
TSVector[] vertices = inTriangle.Vertices;
TSVector vertex0 = vertices[0];
TSVector vertex1 = vertices[1];
TSVector vertex2 = vertices[2];
TSVector edge1, edge2, h, s, q;
FP a, f, u, v;
edge1 = inTriangle.edge1;
edge2 = inTriangle.edge2;
h = TSVector.Cross(direction, edge2);
a = TSVector.Dot(edge1, h);
if (a > -EPSILON && a < EPSILON)
{
return(false);
}
f = 1 / a;
s = origin - vertex0;
u = f * (TSVector.Dot(s, h));
if (u < FP.Zero || u > FP.One)
{
return(false);
}
q = TSVector.Cross(s, edge1);
v = f * TSVector.Dot(direction, q);
if (v < FP.Zero || u + v > FP.One)
{
return(false);
}
// At this stage we can compute t to find out where the intersection point is on the line.
fraction = f * TSVector.Dot(edge2, q);
if (fraction > EPSILON) // ray intersection
{
return(true);
}
else // This means that there is a line intersection but not a ray intersection.
{
return(false);
}
}
public static void LookAt(TSVector forward, TSVector upwards, out TSMatrix result)
{
TSVector zaxis = forward; zaxis.Normalize();
TSVector xaxis = TSVector.Cross(upwards, zaxis); xaxis.Normalize();
TSVector yaxis = TSVector.Cross(zaxis, xaxis);
result.M11 = xaxis.x;
result.M21 = yaxis.x;
result.M31 = zaxis.x;
result.M12 = xaxis.y;
result.M22 = yaxis.y;
result.M32 = zaxis.y;
result.M13 = xaxis.z;
result.M23 = yaxis.z;
result.M33 = zaxis.z;
}
/// Test if point p and d lie on opposite sides of plane through abc
public int PointOutsideOfPlane(TSVector p, TSVector a, TSVector b, TSVector c, TSVector d)
{
TSVector normal = TSVector.Cross(b - a, c - a);
FP signp = TSVector.Dot(p - a, normal); // [AP AB AC]
FP signd = TSVector.Dot(d - a, normal); // [AD AB AC]
//if (CatchDegenerateTetrahedron)
if (signd * signd < (FP.EN8))
{
return(-1);
}
// Points on opposite sides if expression signs are opposite
return(signp * signd < FP.Zero ? 1 : 0);
}
/// <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>
/// 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);
}
public LimitHingeJoint3D(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis, FP minLimit, FP maxLimit) : base(world, body1, body2, position, hingeAxis)
{
TSVector perpDir = TSVector.up;
if (TSVector.Dot(perpDir, hingeAxis) > 0.1f)
{
perpDir = TSVector.right;
}
TSVector sideAxis = TSVector.Cross(hingeAxis, perpDir);
perpDir = TSVector.Cross(sideAxis, hingeAxis);
perpDir.Normalize();
FP len = 15;
TSVector hingeRelAnchorPos0 = perpDir * len;
FP angleToMiddle = FP.Half * (minLimit - maxLimit);
TSMatrix outMatrix;
TSMatrix.CreateFromAxisAngle(ref hingeAxis, -angleToMiddle * FP.Deg2Rad, out outMatrix);
TSVector hingeRelAnchorPos1 = TSVector.Transform(hingeRelAnchorPos0, outMatrix);
FP hingeHalfAngle = FP.Half * (minLimit + maxLimit);
FP allowedDistance = len * 2 * FP.Sin(hingeHalfAngle * FP.Half * FP.Deg2Rad);
TSVector hingePos = body1.TSPosition;
TSVector relPos0c = hingePos + hingeRelAnchorPos0;
TSVector relPos1c = hingePos + hingeRelAnchorPos1;
distance = new PointPointDistance((RigidBody)body1, (RigidBody)body2, relPos0c, relPos1c);
distance.Distance = allowedDistance;
distance.Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance;
StateTracker.AddTracking(distance);
}
/// <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>
/// 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();
}
}
/// <summary>
/// Solves the contact iteratively.
/// </summary>
public void Iterate()
{
//body1.linearVelocity = JVector.Zero;
//body2.linearVelocity = JVector.Zero;
//return;
if (body1.isStatic && body2.isStatic)
{
return;
}
var dv = body2.linearVelocity - body1.linearVelocity;
if (!body1IsMassPoint)
{
dv -= TSVector.Cross(body1.angularVelocity, relativePos1);
}
if (!body2IsMassPoint)
{
dv += TSVector.Cross(body2.angularVelocity, relativePos2);
}
// this gets us some performance
if (dv.sqrMagnitude < settings.minVelocity * settings.minVelocity)
{
return;
}
FP vn = TSVector.Dot(normal, dv);
FP normalImpulse = massNormal * (-vn + restitutionBias + speculativeVelocity);
FP oldNormalImpulse = accumulatedNormalImpulse;
accumulatedNormalImpulse = oldNormalImpulse + normalImpulse;
if (accumulatedNormalImpulse < FP.Zero)
{
accumulatedNormalImpulse = FP.Zero;
}
normalImpulse = accumulatedNormalImpulse - oldNormalImpulse;
FP vt = TSVector.Dot(dv, tangent);
FP maxTangentImpulse = friction * accumulatedNormalImpulse;
FP tangentImpulse = massTangent * -vt;
FP oldTangentImpulse = accumulatedTangentImpulse;
accumulatedTangentImpulse = oldTangentImpulse + tangentImpulse;
if (accumulatedTangentImpulse < -maxTangentImpulse)
{
accumulatedTangentImpulse = -maxTangentImpulse;
}
else if (accumulatedTangentImpulse > maxTangentImpulse)
{
accumulatedTangentImpulse = maxTangentImpulse;
}
tangentImpulse = accumulatedTangentImpulse - oldTangentImpulse;
// Apply contact impulse
TSVector impulse = normal * normalImpulse + tangent * tangentImpulse;
ApplyImpulse(impulse);
}
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);
}
/// <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;
}
}
}
}
}
/// <summary>
/// PrepareForIteration has to be called before <see cref="Iterate"/>.
/// </summary>
/// <param name="timestep">The timestep of the simulation.</param>
public void PrepareForIteration(FP timestep)
{
TSVector dv = CalculateRelativeVelocity();
FP kNormal = FP.Zero;
TSVector rantra = TSVector.zero;
if (!treatBody1AsStatic)
{
kNormal += body1.inverseMass;
if (!body1IsMassPoint)
{
TSVector.Cross(ref relativePos1, ref normal, out rantra);
TSVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
TSVector.Cross(ref rantra, ref relativePos1, out rantra);
}
}
TSVector rbntrb = TSVector.zero;
if (!treatBody2AsStatic)
{
kNormal += body2.inverseMass;
if (!body2IsMassPoint)
{
TSVector.Cross(ref relativePos2, ref normal, out rbntrb);
TSVector.Transform(ref rbntrb, ref body2.invInertiaWorld, out rbntrb);
TSVector.Cross(ref rbntrb, ref relativePos2, out rbntrb);
}
}
if (!treatBody1AsStatic)
{
kNormal += TSVector.Dot(ref rantra, ref normal);
}
if (!treatBody2AsStatic)
{
kNormal += TSVector.Dot(ref rbntrb, ref normal);
}
massNormal = FP.One / kNormal;
tangent = dv - TSVector.Dot(dv, normal) * normal;
tangent.Normalize();
FP kTangent = FP.Zero;
if (treatBody1AsStatic)
{
rantra.MakeZero();
}
else
{
kTangent += body1.inverseMass;
if (!body1IsMassPoint)
{
TSVector.Cross(ref relativePos1, ref normal, out rantra);
TSVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
TSVector.Cross(ref rantra, ref relativePos1, out rantra);
}
}
if (treatBody2AsStatic)
{
rbntrb.MakeZero();
}
else
{
kTangent += body2.inverseMass;
if (!body2IsMassPoint)
{
TSVector.Cross(ref relativePos2, ref tangent, out rbntrb);
TSVector.Transform(ref rbntrb, ref body2.invInertiaWorld, out rbntrb);
TSVector.Cross(ref rbntrb, ref relativePos2, out rbntrb);
}
}
if (!treatBody1AsStatic)
{
kTangent += TSVector.Dot(ref rantra, ref tangent);
}
if (!treatBody2AsStatic)
{
kTangent += TSVector.Dot(ref rbntrb, ref tangent);
}
massTangent = FP.One / kTangent;
restitutionBias = lostSpeculativeBounce;
speculativeVelocity = FP.Zero;
FP relNormalVel = TSVector.Dot(ref normal, ref dv);
if (Penetration > settings.allowedPenetration)
{
restitutionBias = settings.bias * (FP.One / timestep) * TSMath.Max(FP.Zero, Penetration - settings.allowedPenetration);
restitutionBias = TSMath.Clamp(restitutionBias, FP.Zero, settings.maximumBias);
// body1IsMassPoint = body2IsMassPoint = false;
}
FP timeStepRatio = timestep / lastTimeStep;
accumulatedNormalImpulse *= timeStepRatio;
accumulatedTangentImpulse *= timeStepRatio;
{
// Static/Dynamic friction
FP relTangentVel = -TSVector.Dot(ref tangent, ref dv);
FP tangentImpulse = massTangent * relTangentVel;
FP maxTangentImpulse = -staticFriction * accumulatedNormalImpulse;
if (tangentImpulse < maxTangentImpulse)
{
friction = dynamicFriction;
}
else
{
friction = staticFriction;
}
}
TSVector impulse;
// Simultaneos solving and restitution is simply not possible
// so fake it a bit by just applying restitution impulse when there
// is a new contact.
//同时求解和恢复是不可能的，所以当有新的接触时，仅仅应用恢复脉冲，有点虚假。
if (relNormalVel < -FP.One && newContact)
{
restitutionBias = TSMath.Max(-restitution * relNormalVel, restitutionBias);
}
// Speculative Contacts!
// if the penetration is negative (which means the bodies are not already in contact, but they will
// be in the future) we store the current bounce bias in the variable 'lostSpeculativeBounce'
// and apply it the next frame, when the speculative contact was already solved.
// 如果穿透是负的（这意味着物体还没有接触，但是它们将来会接触），
// 我们将当前弹跳偏置存储在变量“lostSpeculativeBounce”中，并在下一个框架中应用它，此时投机接触已经解决。
if (penetration < -settings.allowedPenetration)
{
speculativeVelocity = penetration / timestep;
lostSpeculativeBounce = restitutionBias;
restitutionBias = FP.Zero;
}
else
{
lostSpeculativeBounce = FP.Zero;
}
impulse = normal * accumulatedNormalImpulse + tangent * accumulatedTangentImpulse;
ApplyImpulse(ref impulse);
lastTimeStep = timestep;
newContact = false;
}
/// <summary>
/// Calculates the cross product of two vectors.
/// </summary>
/// <param name="value1">The first vector.</param>
/// <param name="value2">The second vector.</param>
/// <returns>Returns the cross product of both.</returns>
#region public static JVector operator %(JVector value1, JVector value2)
public static TSVector operator %(TSVector value1, TSVector value2)
{
TSVector result; TSVector.Cross(ref value1, ref value2, out result);
return(result);
}
