public void OnDrawGizmos()
{
if (LocalDirection.Equals(Vector3.zero))
{
return;
}
var originalColor = Gizmos.color;
var originalMatrix = Gizmos.matrix;
Gizmos.color = Color.red;
// Calculate the final Physics Body runtime coordinate system which bakes out skew from non-uniform scaling in parent
var worldFromLocalRigidTransform = Math.DecomposeRigidBodyTransform(transform.localToWorldMatrix);
var worldFromLocal = Matrix4x4.TRS(worldFromLocalRigidTransform.pos, worldFromLocalRigidTransform.rot, Vector3.one);
Vector3 directionWorld = worldFromLocal.MultiplyVector(LocalDirection.normalized);
Vector3 offsetWorld = worldFromLocal.MultiplyPoint(LocalOffset);
// Calculate the final world Thrust coordinate system from the world Body transform and local offset and direction
Math.CalculatePerpendicularNormalized(directionWorld, out _, out var directionPerpendicular);
var worldFromThrust = Matrix4x4.TRS(offsetWorld, Quaternion.LookRotation(directionWorld, directionPerpendicular), Vector3.one);
Gizmos.matrix = worldFromThrust;
float Shift = Magnitude * 0.1f;
Gizmos.DrawFrustum(new Vector3(0, 0, -Shift), UnityEngine.Random.Range(1.0f, 2.5f), Magnitude, Shift, 1.0f);
Gizmos.matrix = originalMatrix;
Gizmos.color = originalColor;
}
public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, float timestep, float invTimestep)
{
// Predict the relative orientation at the end of the step
quaternion futureBFromA = JacobianUtilities.IntegrateOrientationBFromA(BFromA, velocityA.AngularVelocity, velocityB.AngularVelocity, timestep);
// Find the future axis and angle of rotation between the free axes
float3 jacA0, jacA1, jacA2, jacB0, jacB1, jacB2;
float3 effectiveMass; // first column of 3x3 effective mass matrix, don't need the others because only jac0 can have nonzero error
float futureAngle;
{
// Calculate the relative rotation between joint spaces
quaternion jointOrientation = math.mul(math.inverse(RefBFromA), futureBFromA);
// Find the axis and angle of rotation
jacA0 = jointOrientation.value.xyz;
float sinHalfAngleSq = math.lengthsq(jacA0);
float invSinHalfAngle = Math.RSqrtSafe(sinHalfAngleSq);
float sinHalfAngle = sinHalfAngleSq * invSinHalfAngle;
futureAngle = math.asin(sinHalfAngle) * 2.0f;
jacA0 = math.select(jacA0 * invSinHalfAngle, new float3(1, 0, 0), invSinHalfAngle == 0.0f);
jacA0 = math.select(jacA0, -jacA0, jointOrientation.value.w < 0.0f);
Math.CalculatePerpendicularNormalized(jacA0, out jacA1, out jacA2);
jacB0 = math.mul(futureBFromA, -jacA0);
jacB1 = math.mul(futureBFromA, -jacA1);
jacB2 = math.mul(futureBFromA, -jacA2);
// Calculate the effective mass
float3 invEffectiveMassDiag = new float3(
math.csum(jacA0 * jacA0 * velocityA.InverseInertiaAndMass.xyz + jacB0 * jacB0 * velocityB.InverseInertiaAndMass.xyz),
math.csum(jacA1 * jacA1 * velocityA.InverseInertiaAndMass.xyz + jacB1 * jacB1 * velocityB.InverseInertiaAndMass.xyz),
math.csum(jacA2 * jacA2 * velocityA.InverseInertiaAndMass.xyz + jacB2 * jacB2 * velocityB.InverseInertiaAndMass.xyz));
float3 invEffectiveMassOffDiag = new float3(
math.csum(jacA0 * jacA1 * velocityA.InverseInertiaAndMass.xyz + jacB0 * jacB1 * velocityB.InverseInertiaAndMass.xyz),
math.csum(jacA0 * jacA2 * velocityA.InverseInertiaAndMass.xyz + jacB0 * jacB2 * velocityB.InverseInertiaAndMass.xyz),
math.csum(jacA1 * jacA2 * velocityA.InverseInertiaAndMass.xyz + jacB1 * jacB2 * velocityB.InverseInertiaAndMass.xyz));
JacobianUtilities.InvertSymmetricMatrix(invEffectiveMassDiag, invEffectiveMassOffDiag, out float3 effectiveMassDiag, out float3 effectiveMassOffDiag);
effectiveMass = JacobianUtilities.BuildSymmetricMatrix(effectiveMassDiag, effectiveMassOffDiag).c0;
}
// Calculate the error, adjust by tau and damping, and apply an impulse to correct it
float futureError = JacobianUtilities.CalculateError(futureAngle, MinAngle, MaxAngle);
float solveError = JacobianUtilities.CalculateCorrection(futureError, InitialError, Tau, Damping);
float solveVelocity = -solveError * invTimestep;
float3 impulseA = solveVelocity * (jacA0 * effectiveMass.x + jacA1 * effectiveMass.y + jacA2 * effectiveMass.z);
float3 impulseB = solveVelocity * (jacB0 * effectiveMass.x + jacB1 * effectiveMass.y + jacB2 * effectiveMass.z);
velocityA.ApplyAngularImpulse(impulseA);
velocityB.ApplyAngularImpulse(impulseB);
}
public static unsafe Result CapsuleCapsule(CapsuleCollider *capsuleA, CapsuleCollider *capsuleB, MTransform aFromB)
{
// Transform capsule B into A-space
float3 pointB = Mul(aFromB, capsuleB->Vertex0);
float3 edgeB = math.mul(aFromB.Rotation, capsuleB->Vertex1 - capsuleB->Vertex0);
// Get point and edge of A
float3 pointA = capsuleA->Vertex0;
float3 edgeA = capsuleA->Vertex1 - capsuleA->Vertex0;
// Get the closest points on the capsules
SegmentSegment(pointA, edgeA, pointB, edgeB, out float3 closestA, out float3 closestB);
float3 diff = closestA - closestB;
float coreDistanceSq = math.lengthsq(diff);
if (coreDistanceSq < distanceEpsSq)
{
// Special case for extremely small distances, should be rare
float3 normal = math.cross(edgeA, edgeB);
if (math.lengthsq(normal) < 1e-5f)
{
float3 edge = math.normalizesafe(edgeA, math.normalizesafe(edgeB, new float3(1, 0, 0))); // edges are parallel or one of the capsules is a sphere
Math.CalculatePerpendicularNormalized(edge, out normal, out float3 unused); // normal is anything perpendicular to edge
}
else
{
normal = math.normalize(normal); // normal is cross of edges, sign doesn't matter
}
return(new Result
{
NormalInA = normal,
PositionOnAinA = pointA - normal * capsuleA->Radius,
Distance = -capsuleA->Radius - capsuleB->Radius
});
}
return(PointPoint(closestA, closestB, capsuleA->Radius, capsuleA->Radius + capsuleB->Radius));
}
protected override JobHandle OnUpdate(JobHandle inputDeps)
{
if (m_ContactModifierGroup.CalculateLength() == 0)
{
return(inputDeps);
}
if (m_StepPhysicsWorld.Simulation.Type == SimulationType.NoPhysics)
{
return(inputDeps);
}
SimulationCallbacks.Callback preparationCallback = (ref ISimulation simulation, JobHandle inDeps) =>
{
inDeps.Complete(); // shouldn't be needed (jobify the below)
SimulationData.Contacts.Iterator iterator = simulation.Contacts.GetIterator();
while (iterator.HasItemsLeft())
{
ContactHeader manifold = iterator.GetNextContactHeader();
// JacobianModifierFlags format for this example
// UserData 0 - soft contact
// UserData 1 - surface velocity
// UserData 2 - infinite inertia
// UserData 3 - no torque
// UserData 4 - clip impulse
// UserData 5 - disabled contact
if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 0)) ||
0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 0)))
{
manifold.JacobianFlags |= JacobianFlags.UserFlag0; // Soft Contact
}
if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 1)) ||
0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 1)))
{
manifold.JacobianFlags |= JacobianFlags.EnableSurfaceVelocity;
}
if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 2)) ||
0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 2)))
{
manifold.JacobianFlags |= JacobianFlags.EnableMassFactors;
}
if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 3)) ||
0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 3)))
{
manifold.JacobianFlags |= JacobianFlags.UserFlag1; // No Torque
}
if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 4)) ||
0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 4)))
{
manifold.JacobianFlags |= JacobianFlags.EnableMaxImpulse; // No Torque
}
if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 5)) ||
0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 5)))
{
manifold.JacobianFlags |= JacobianFlags.Disabled;
}
iterator.UpdatePreviousContactHeader(manifold);
// Just read contacts
for (int i = 0; i < manifold.NumContacts; i++)
{
iterator.GetNextContact();
}
}
return(inDeps);
};
SimulationCallbacks.Callback jacobianModificationCallback = (ref ISimulation simulation, JobHandle inDeps) =>
{
inDeps.Complete(); // shouldn't be needed (jobify the below)
JacobianIterator iterator = simulation.Jacobians.Iterator;
while (iterator.HasJacobiansLeft())
{
// JacobianModifierFlags format for this example
// UserFlag0 - soft contact
// UserFlag1 - no torque
// Jacobian header
ref JacobianHeader jacHeader = ref iterator.ReadJacobianHeader();
// Triggers can only be disabled, other modifiers have no effect
if (jacHeader.Type == JacobianType.Contact)
{
// Contact jacobian modification
ref ContactJacobian contactJacobian = ref jacHeader.AccessBaseJacobian <ContactJacobian>();
{
// Check if NoTorque modifier
if ((jacHeader.Flags & JacobianFlags.UserFlag1) != 0)
{
// Disable all friction angular effects
contactJacobian.Friction0.AngularA = 0.0f;
contactJacobian.Friction1.AngularA = 0.0f;
contactJacobian.AngularFriction.AngularA = 0.0f;
contactJacobian.Friction0.AngularB = 0.0f;
contactJacobian.Friction1.AngularB = 0.0f;
contactJacobian.AngularFriction.AngularB = 0.0f;
}
// Check if SurfaceVelocity present
if (jacHeader.HasSurfaceVelocity)
{
// Since surface normal can change, make sure angular velocity is always relative to it, not independent
float3 angVel = contactJacobian.BaseJacobian.Normal * (new float3(0.0f, 1.0f, 0.0f));
float3 linVel = float3.zero;
Math.CalculatePerpendicularNormalized(contactJacobian.BaseJacobian.Normal, out float3 dir0, out float3 dir1);
float linVel0 = math.dot(linVel, dir0);
float linVel1 = math.dot(linVel, dir1);
float angVelProj = math.dot(angVel, contactJacobian.BaseJacobian.Normal);
jacHeader.SurfaceVelocity = new SurfaceVelocity {
ExtraFrictionDv = new float3(linVel0, linVel1, angVelProj)
};
}
public void Execute(ref ModifiableJacobianHeader jacHeader, ref ModifiableContactJacobian contactJacobian)
{
Entity entityA = jacHeader.Entities.EntityA;
Entity entityB = jacHeader.Entities.EntityB;
ModifyContactJacobians.ModificationType typeA = ModifyContactJacobians.ModificationType.None;
if (modificationData.Exists(entityA))
{
typeA = modificationData[entityA].type;
}
ModifyContactJacobians.ModificationType typeB = ModifyContactJacobians.ModificationType.None;
if (modificationData.Exists(entityB))
{
typeB = modificationData[entityB].type;
}
{
// Check for jacobians we want to ignore:
if (typeA == ModifyContactJacobians.ModificationType.DisabledContact || typeB == ModifyContactJacobians.ModificationType.DisabledContact)
{
jacHeader.Flags = jacHeader.Flags | JacobianFlags.Disabled;
}
// Check if NoTorque modifier
if (typeA == ModifyContactJacobians.ModificationType.NoAngularEffects || typeB == ModifyContactJacobians.ModificationType.NoAngularEffects)
{
// Disable all friction angular effects
var friction0 = contactJacobian.Friction0;
friction0.AngularA = 0.0f;
friction0.AngularB = 0.0f;
contactJacobian.Friction0 = friction0;
var friction1 = contactJacobian.Friction1;
friction1.AngularA = 0.0f;
friction1.AngularB = 0.0f;
contactJacobian.Friction1 = friction1;
var angularFriction = contactJacobian.AngularFriction;
angularFriction.AngularA = 0.0f;
angularFriction.AngularB = 0.0f;
contactJacobian.AngularFriction = angularFriction;
}
// Check if SurfaceVelocity present
if (jacHeader.HasSurfaceVelocity &&
(typeA == ModifyContactJacobians.ModificationType.SurfaceVelocity || typeB == ModifyContactJacobians.ModificationType.SurfaceVelocity))
{
// Since surface normal can change, make sure angular velocity is always relative to it, not independent
float3 angVel = contactJacobian.Normal * (new float3(0.0f, 1.0f, 0.0f));
float3 linVel = float3.zero;
Math.CalculatePerpendicularNormalized(contactJacobian.Normal, out float3 dir0, out float3 dir1);
float linVel0 = math.dot(linVel, dir0);
float linVel1 = math.dot(linVel, dir1);
float angVelProj = math.dot(angVel, contactJacobian.Normal);
//jacHeader.SurfaceVelocity = new SurfaceVelocity { ExtraFrictionDv = new float3(linVel0, linVel1, angVelProj) };
}
/*
* // Check if MaxImpulse present
* if (jacHeader.HasMaxImpulse &&
* (typeA == ModifyContactJacobians.ModificationType.ClippedImpulse || typeB == ModifyContactJacobians.ModificationType.ClippedImpulse))
* {
* // Max impulse in Ns (Newton-second)
* jacHeader.MaxImpulse = 20.0f;
* }
*/
// Check if MassFactors present
if (jacHeader.HasMassFactors &&
(typeA == ModifyContactJacobians.ModificationType.InfiniteInertia || typeB == ModifyContactJacobians.ModificationType.InfiniteInertia))
{
// Give both bodies infinite inertia
jacHeader.MassFactors = new MassFactors
{
InvInertiaAndMassFactorA = new float4(0.0f, 0.0f, 0.0f, 1.0f),
InvInertiaAndMassFactorB = new float4(0.0f, 0.0f, 0.0f, 1.0f)
};
}
}
// Angular jacobian modifications
for (int i = 0; i < contactJacobian.NumContacts; i++)
{
ContactJacAngAndVelToReachCp jacobianAngular = jacHeader.GetAngularJacobian(i);
// Check if NoTorque modifier
if (typeA == ModifyContactJacobians.ModificationType.NoAngularEffects || typeB == ModifyContactJacobians.ModificationType.NoAngularEffects)
{
// Disable all angular effects
jacobianAngular.Jac.AngularA = 0.0f;
jacobianAngular.Jac.AngularB = 0.0f;
}
// Check if SoftContact modifier
if (typeA == ModifyContactJacobians.ModificationType.SoftContact || typeB == ModifyContactJacobians.ModificationType.SoftContact)
{
jacobianAngular.Jac.EffectiveMass *= 0.1f;
if (jacobianAngular.VelToReachCp > 0.0f)
{
jacobianAngular.VelToReachCp *= 0.5f;
}
}
jacHeader.SetAngularJacobian(i, jacobianAngular);
}
}
/// <summary>
/// Generalized convex-convex distance.
/// </summary>
/// <param name="verticesA">Vertices of the first collider in local space</param>
/// <param name="verticesB">Vertices of the second collider in local space</param>
/// <param name="aFromB">Transform from the local space of B to the local space of A</param>
/// <param name="penetrationHandling">How to compute penetration.</param>
/// <returns></returns>
public static unsafe Result ConvexConvex(
float3 *verticesA, int numVerticesA, float3 *verticesB, int numVerticesB,
MTransform aFromB, PenetrationHandling penetrationHandling)
{
const float epsTerminationSq = 1e-8f; // Main loop quits when it cannot find a point that improves the simplex by at least this much
const float epsPenetrationSq = 1e-9f; // Epsilon used to check for penetration. Should be smaller than shape cast ConvexConvex keepDistance^2.
// Initialize simplex.
Simplex simplex = new Simplex();
simplex.NumVertices = 1;
simplex.A = GetSupportingVertex(new float3(1, 0, 0), verticesA, numVerticesA, verticesB, numVerticesB, aFromB);
simplex.Direction = simplex.A.Xyz;
simplex.ScaledDistance = math.lengthsq(simplex.A.Xyz);
float scaleSq = simplex.ScaledDistance;
// Iterate.
int iteration = 0;
bool penetration = false;
const int maxIterations = 64;
for (; iteration < maxIterations; ++iteration)
{
// Find a new support vertex
SupportVertex newSv = GetSupportingVertex(-simplex.Direction, verticesA, numVerticesA, verticesB, numVerticesB, aFromB);
// If the new vertex is not significantly closer to the origin, quit
float scaledImprovement = math.dot(simplex.A.Xyz - newSv.Xyz, simplex.Direction);
if (scaledImprovement * math.abs(scaledImprovement) < epsTerminationSq * scaleSq)
{
break;
}
// Add the new vertex and reduce the simplex
switch (simplex.NumVertices++)
{
case 1: simplex.B = newSv; break;
case 2: simplex.C = newSv; break;
default: simplex.D = newSv; break;
}
simplex.SolveDistance();
// Check for penetration
scaleSq = math.lengthsq(simplex.Direction);
float scaledDistanceSq = simplex.ScaledDistance * simplex.ScaledDistance;
if (simplex.NumVertices == 4 || scaledDistanceSq <= epsPenetrationSq * scaleSq)
{
penetration = true;
break;
}
}
// Finalize result.
var ret = new Result {
Iterations = iteration + 1
};
// Handle penetration.
if (penetration && penetrationHandling != PenetrationHandling.DotNotCompute)
{
// Allocate a hull for EPA
int verticesCapacity = 64;
int triangleCapacity = 2 * verticesCapacity;
ConvexHullBuilder.Vertex * vertices = stackalloc ConvexHullBuilder.Vertex[verticesCapacity];
ConvexHullBuilder.Triangle *triangles = stackalloc ConvexHullBuilder.Triangle[triangleCapacity];
var hull = new ConvexHullBuilder(vertices, verticesCapacity, triangles, triangleCapacity);
// Initialize int space
// TODO - either the hull should be robust when int space changes after points are already added, or the ability to do so should be removed, probably the latter
// Currently for example a valid triangle can collapse to a line segment when the bounds grow
hull.IntegerSpaceAabb = GetSupportingAabb(verticesA, numVerticesA, verticesB, numVerticesB, aFromB);
// Add simplex vertices to the hull, remove any vertices from the simplex that do not increase the hull dimension
hull.AddPoint(simplex.A.Xyz, simplex.A.Id);
if (simplex.NumVertices > 1)
{
hull.AddPoint(simplex.B.Xyz, simplex.B.Id);
if (simplex.NumVertices > 2)
{
int dimension = hull.Dimension;
hull.AddPoint(simplex.C.Xyz, simplex.C.Id);
if (dimension == 0 && hull.Dimension == 1)
{
simplex.B = simplex.C;
}
if (simplex.NumVertices > 3)
{
dimension = hull.Dimension;
hull.AddPoint(simplex.D.Xyz, simplex.D.Id);
if (dimension > hull.Dimension)
{
if (dimension == 0)
{
simplex.B = simplex.D;
}
else if (dimension == 1)
{
simplex.C = simplex.D;
}
}
}
}
}
simplex.NumVertices = (hull.Dimension + 1);
// If the simplex is not 3D, try expanding the hull in all directions
while (hull.Dimension < 3)
{
// Choose expansion directions
float3 support0, support1, support2;
switch (simplex.NumVertices)
{
case 1:
support0 = new float3(1, 0, 0);
support1 = new float3(0, 1, 0);
support2 = new float3(0, 0, 1);
break;
case 2:
Math.CalculatePerpendicularNormalized(math.normalize(simplex.B.Xyz - simplex.A.Xyz), out support0, out support1);
support2 = float3.zero;
break;
default:
UnityEngine.Assertions.Assert.IsTrue(simplex.NumVertices == 3);
support0 = math.cross(simplex.B.Xyz - simplex.A.Xyz, simplex.C.Xyz - simplex.A.Xyz);
support1 = float3.zero;
support2 = float3.zero;
break;
}
// Try each one
int numSupports = 4 - simplex.NumVertices;
bool success = false;
for (int i = 0; i < numSupports; i++)
{
for (int j = 0; j < 2; j++) // +/- each direction
{
SupportVertex vertex = GetSupportingVertex(support0, verticesA, numVerticesA, verticesB, numVerticesB, aFromB);
hull.AddPoint(vertex.Xyz, vertex.Id);
if (hull.Dimension == simplex.NumVertices)
{
switch (simplex.NumVertices)
{
case 1: simplex.B = vertex; break;
case 2: simplex.C = vertex; break;
default: simplex.D = vertex; break;
}
// Next dimension
success = true;
simplex.NumVertices++;
i = numSupports;
break;
}
support0 = -support0;
}
support0 = support1;
support1 = support2;
}
if (!success)
{
break;
}
}
// We can still fail to build a tetrahedron if the minkowski difference is really flat.
// In those cases just find the closest point to the origin on the infinite extension of the simplex (point / line / plane)
if (hull.Dimension != 3)
{
switch (simplex.NumVertices)
{
case 1:
{
ret.ClosestPoints.Distance = math.length(simplex.A.Xyz);
ret.ClosestPoints.NormalInA = -math.normalizesafe(simplex.A.Xyz, new float3(1, 0, 0));
break;
}
case 2:
{
float3 edge = math.normalize(simplex.B.Xyz - simplex.A.Xyz);
float3 direction = math.cross(math.cross(edge, simplex.A.Xyz), edge);
Math.CalculatePerpendicularNormalized(edge, out float3 safeNormal, out float3 unused); // backup, take any direction perpendicular to the edge
float3 normal = math.normalizesafe(direction, safeNormal);
ret.ClosestPoints.Distance = math.dot(normal, simplex.A.Xyz);
ret.ClosestPoints.NormalInA = -normal;
break;
}
default:
{
UnityEngine.Assertions.Assert.IsTrue(simplex.NumVertices == 3);
float3 normal = math.normalize(math.cross(simplex.B.Xyz - simplex.A.Xyz, simplex.C.Xyz - simplex.A.Xyz));
float dot = math.dot(normal, simplex.A.Xyz);
ret.ClosestPoints.Distance = math.abs(dot);
ret.ClosestPoints.NormalInA = math.select(-normal, normal, dot < 0);
break;
}
}
}
else
{
int closestTriangleIndex;
Plane closestPlane = new Plane();
float stopThreshold = 1e-4f;
uint *uidsCache = stackalloc uint[triangleCapacity];
for (int i = 0; i < triangleCapacity; i++)
{
uidsCache[i] = 0;
}
float *distancesCache = stackalloc float[triangleCapacity];
do
{
// Select closest triangle.
closestTriangleIndex = -1;
foreach (int triangleIndex in hull.Triangles.Indices)
{
if (hull.Triangles[triangleIndex].Uid != uidsCache[triangleIndex])
{
uidsCache[triangleIndex] = hull.Triangles[triangleIndex].Uid;
distancesCache[triangleIndex] = hull.ComputePlane(triangleIndex).Distance;
}
if (closestTriangleIndex == -1 || distancesCache[closestTriangleIndex] < distancesCache[triangleIndex])
{
closestTriangleIndex = triangleIndex;
}
}
closestPlane = hull.ComputePlane(closestTriangleIndex);
// Add supporting vertex or exit.
SupportVertex sv = GetSupportingVertex(closestPlane.Normal, verticesA, numVerticesA, verticesB, numVerticesB, aFromB);
float d2P = math.dot(closestPlane.Normal, sv.Xyz) + closestPlane.Distance;
if (math.abs(d2P) > stopThreshold && hull.AddPoint(sv.Xyz, sv.Id))
{
stopThreshold *= 1.3f;
}
else
{
break;
}
} while (++iteration < maxIterations);
// Generate simplex.
ConvexHullBuilder.Triangle triangle = hull.Triangles[closestTriangleIndex];
simplex.NumVertices = 3;
simplex.A.Xyz = hull.Vertices[triangle.Vertex0].Position; simplex.A.Id = hull.Vertices[triangle.Vertex0].UserData;
simplex.B.Xyz = hull.Vertices[triangle.Vertex1].Position; simplex.B.Id = hull.Vertices[triangle.Vertex1].UserData;
simplex.C.Xyz = hull.Vertices[triangle.Vertex2].Position; simplex.C.Id = hull.Vertices[triangle.Vertex2].UserData;
simplex.Direction = -closestPlane.Normal;
simplex.ScaledDistance = closestPlane.Distance;
// Set normal and distance.
ret.ClosestPoints.NormalInA = -closestPlane.Normal;
ret.ClosestPoints.Distance = closestPlane.Distance;
}
}
else
{
// Compute distance and normal.
float lengthSq = math.lengthsq(simplex.Direction);
float invLength = math.rsqrt(lengthSq);
bool smallLength = lengthSq < 1e-10f;
ret.ClosestPoints.Distance = math.select(simplex.ScaledDistance * invLength, 0.0f, smallLength);
ret.ClosestPoints.NormalInA = math.select(simplex.Direction * invLength, new float3(1, 0, 0), smallLength);
// Make sure the normal is always valid.
if (!math.all(math.isfinite(ret.ClosestPoints.NormalInA)))
{
ret.ClosestPoints.NormalInA = new float3(1, 0, 0);
}
}
// Compute position.
float3 closestPoint = ret.ClosestPoints.NormalInA * ret.ClosestPoints.Distance;
float4 coordinates = simplex.ComputeBarycentricCoordinates(closestPoint);
ret.ClosestPoints.PositionOnAinA =
verticesA[simplex.A.IdA] * coordinates.x +
verticesA[simplex.B.IdA] * coordinates.y +
verticesA[simplex.C.IdA] * coordinates.z +
verticesA[simplex.D.IdA] * coordinates.w;
// Encode simplex.
ret.Simplex.x = simplex.A.Id;
ret.Simplex.y = simplex.NumVertices >= 2 ? simplex.B.Id : Result.InvalidSimplexVertex;
ret.Simplex.z = simplex.NumVertices >= 3 ? simplex.C.Id : Result.InvalidSimplexVertex;
// Done.
UnityEngine.Assertions.Assert.IsTrue(math.isfinite(ret.ClosestPoints.Distance));
UnityEngine.Assertions.Assert.IsTrue(math.abs(math.lengthsq(ret.ClosestPoints.NormalInA) - 1.0f) < 1e-5f);
return(ret);
}
public unsafe static void Execute(ref JacobiansJobData <T> jobData, IntPtr additionalData,
IntPtr bufferRangePatchData, ref JobRanges ranges, int jobIndex)
{
ModifiableJacobianHeader modifiableHeader;
ModifiableContactJacobian modifiableContact;
byte *headerBuffer = stackalloc byte[JacobianHeader.CalculateSize(JacobianType.Contact, (JacobianFlags)0xff, 4)]; //<todo.eoin.modifier How to verify correct sizes?
byte *contactBuffer = stackalloc byte[sizeof(ContactJacobian)];
Havok.Physics.HpGrid *curGrid = jobData.FixedJacobianGrid;
int *pluginIndexToLocal = jobData.PluginIndexToLocal->Data;
for (int g = 0; g < 2; g++)
{
for (int i = 0; i < curGrid->m_size; i++)
{
HpCsContactJacRange *gridRange = curGrid->m_entries + i;
var range = (Havok.Physics.HpLinkedRange *)UnsafeUtility.AddressOf(ref gridRange->m_range);
while (range != null)
{
var reader = new Havok.Physics.HpBlockStreamReader(range);
while (reader.HasItems)
{
var hpHeader = (Havok.Physics.HpJacHeader *)reader.Peek();
modifiableHeader = new ModifiableJacobianHeader {
};
modifiableContact = new ModifiableContactJacobian {
};
modifiableHeader.m_Header = (JacobianHeader *)headerBuffer;
modifiableContact.m_ContactJacobian = (ContactJacobian *)contactBuffer;
int bodyIndexA = pluginIndexToLocal[hpHeader->m_bodyIdA & 0x00ffffff];
int bodyIndexB = pluginIndexToLocal[hpHeader->m_bodyIdB & 0x00ffffff];
modifiableHeader.m_Header->BodyPair = new BodyIndexPair
{
BodyIndexA = bodyIndexA,
BodyIndexB = bodyIndexB
};
modifiableHeader.EntityPair = new EntityPair
{
EntityA = jobData.Bodies[bodyIndexA].Entity,
EntityB = jobData.Bodies[bodyIndexB].Entity
};
modifiableHeader.m_Header->Type = JacobianType.Contact;
modifiableContact.m_ContactJacobian->BaseJacobian.NumContacts = hpHeader->m_numPoints;
modifiableContact.m_ContactJacobian->BaseJacobian.Normal = hpHeader->m_normal.xyz;
Havok.Physics.HPManifoldCollisionCache *manifoldCache = hpHeader->m_manifoldCollisionCache;
modifiableContact.m_ContactJacobian->CoefficientOfFriction = manifoldCache->m_friction.Value;
// Fill in friction data
if (HpJacHeader.hasAnyFriction(hpHeader->m_flagsAndDimB))
{
Havok.Physics.HpJac3dFriction *jf = hpHeader->accessJacFriction();
modifiableContact.m_ContactJacobian->Friction0.AngularA = jf->m_jacDir0_angular0.xyz;
modifiableContact.m_ContactJacobian->Friction0.AngularB = jf->m_jacDir0_angular1.xyz;
modifiableContact.m_ContactJacobian->Friction1.AngularA = jf->m_jacDir1_angular0.xyz;
modifiableContact.m_ContactJacobian->Friction1.AngularB = jf->m_jacDir1_angular1.xyz;
modifiableContact.m_ContactJacobian->AngularFriction.AngularA = jf->m_jacAng_angular0.xyz;
modifiableContact.m_ContactJacobian->AngularFriction.AngularB = jf->m_jacAng_angular1.xyz;
}
Havok.Physics.HpPerManifoldProperty *cdp = manifoldCache->GetCustomPropertyStorage();
modifiableHeader.m_Header->Flags = (JacobianFlags)cdp->m_jacobianFlags;
if ((cdp->m_jacobianFlags & (byte)JacobianFlags.EnableMassFactors) != 0)
{
modifiableHeader.MassFactors = *hpHeader->accessMassFactors();
}
for (int p = 0; p < hpHeader->m_numPoints; p++)
{
Havok.Physics.HpJacAngular *hpAng = hpHeader->accessJacAngular(p);
var ang = new ContactJacAngAndVelToReachCp
{
Jac = new ContactJacobianAngular
{
AngularA = hpAng->m_angular0.xyz,
AngularB = hpAng->m_angular1.xyz,
EffectiveMass = hpAng->m_angular0.w,
},
VelToReachCp = hpAng->m_angular1.w,
};
// Access the angular jacobian from the header directly,
// to avoid the modifiable header marking itself dirty.
modifiableHeader.m_Header->AccessAngularJacobian(p) = ang;
}
jobData.UserJobData.Execute(ref modifiableHeader, ref modifiableContact);
if (((byte)modifiableHeader.Flags & (byte)JacobianFlags.Disabled) != 0)
{
// Don't check the "changed" state of the jacobian - this flag is set on the contact
hpHeader->m_flagsAndDimB |= 1 << 10; // JH_MANIFOLD_IS_NOT_NORMAL
hpHeader->m_manifoldType = 3; // hknpManifoldType::DISABLED
}
if (modifiableHeader.AngularChanged || modifiableHeader.ModifiersChanged)
{
// Need to disable jacobian caching, since we can't tell what modifications the user has done
manifoldCache->m_qualityFlags &= (0xffff ^ (1 << 10)); //hknpBodyQuality::ENABLE_CONTACT_CACHING
}
if (modifiableHeader.AngularChanged)
{
for (int p = 0; p < hpHeader->m_numPoints; p++)
{
Havok.Physics.HpJacAngular * hpAng = hpHeader->accessJacAngular(p);
ContactJacAngAndVelToReachCp ang = modifiableHeader.GetAngularJacobian(p);
hpAng->m_angular0 = new float4(ang.Jac.AngularA, ang.Jac.EffectiveMass);
hpAng->m_angular1 = new float4(ang.Jac.AngularB, ang.VelToReachCp);
}
}
if (modifiableHeader.ModifiersChanged && (cdp->m_jacobianFlags & (byte)JacobianFlags.EnableMassFactors) != 0)
{
*hpHeader->accessMassFactors() = modifiableHeader.MassFactors;
}
if (modifiableHeader.ModifiersChanged && (cdp->m_jacobianFlags & (byte)JacobianFlags.EnableSurfaceVelocity) != 0)
{
var surfVel = modifiableHeader.SurfaceVelocity;
float angVelProj = math.dot(surfVel.AngularVelocity, modifiableContact.Normal);
if (manifoldCache != null)
{
float frictionRhs = manifoldCache->getFrictionRhsMultiplierValue();
float frictRhsMul = frictionRhs * jobData.TimeStep;
// Update cached integrated friction rhs
float4 vel = new float4(-surfVel.LinearVelocity, -angVelProj);
float4 rhs4 = manifoldCache->m_integratedFrictionRhs;
rhs4 += frictRhsMul * vel;
manifoldCache->m_integratedFrictionRhs = rhs4;
}
if (HpJacHeader.hasAnyFriction(hpHeader->m_flagsAndDimB))
{
Math.CalculatePerpendicularNormalized(modifiableContact.Normal, out float3 dir0, out float3 dir1);
float linVel0 = math.dot(surfVel.LinearVelocity, dir0);
float linVel1 = math.dot(surfVel.LinearVelocity, dir1);
// Check JH_SURFACE_VELOCITY_DIRTY flag and clear it if it was set
const ushort jhSurfaceVelocityDirty = 1 << 3;
if ((hpHeader->m_flagsAndDimB & jhSurfaceVelocityDirty) != 0)
{
*hpHeader->accessSurfaceVelocity() = new float3(linVel0, linVel1, angVelProj);
hpHeader->m_flagsAndDimB &= 0xffff ^ jhSurfaceVelocityDirty;
}
else
{
*hpHeader->accessSurfaceVelocity() += new float3(linVel0, linVel1, angVelProj);
}
// Update friction Jacobian
{
Havok.Physics.HpJac3dFriction *jf = hpHeader->accessJacFriction();
float dRhs0 = jf->m_jacDir0_linear0.w;
float dRhs1 = jf->m_jacDir1_linear0.w;
float dRhs = jf->m_jacAng_angular1.w;
float frictRhsMul = hpHeader->m_manifoldCollisionCache->getFrictionRhsMultiplierValue();
dRhs0 -= frictRhsMul * linVel0;
dRhs1 -= frictRhsMul * linVel1;
dRhs -= frictRhsMul * angVelProj;
jf->m_jacDir0_linear0.w = dRhs0;
jf->m_jacDir1_linear0.w = dRhs1;
jf->m_jacAng_angular1.w = dRhs;
}
}
}
if (modifiableContact.Modified)
{
hpHeader->m_normal.xyz = modifiableContact.Normal;
manifoldCache->m_friction.Value = modifiableContact.CoefficientOfFriction;
if (HpJacHeader.hasAnyFriction(hpHeader->m_flagsAndDimB))
{
Havok.Physics.HpJac3dFriction *jf = hpHeader->accessJacFriction();
jf->m_jacDir0_angular0.xyz = modifiableContact.Friction0.AngularA;
jf->m_jacDir0_angular1.xyz = modifiableContact.Friction0.AngularB;
jf->m_jacDir1_angular0.xyz = modifiableContact.Friction1.AngularA;
jf->m_jacDir1_angular1.xyz = modifiableContact.Friction1.AngularB;
jf->m_jacAng_angular0.xyz = modifiableContact.AngularFriction.AngularA;
jf->m_jacAng_angular1.xyz = modifiableContact.AngularFriction.AngularB;
}
}
reader.Advance(hpHeader->m_sizeDiv16 * 16);
}
range = range->m_next;
}
}
curGrid = jobData.MovingJacobianGrid;
}
}