public void CalculateContactMassesB(BurstRigidbody rigidbody, bool rollingContacts)
{
// initialize inverse linear masses:
normalInvMassB = tangentInvMassB = bitangentInvMassB = rigidbody.inverseMass;
if (rollingContacts)
{
float4 rB = ContactPointB - rigidbody.com;
normalInvMassB += BurstMath.RotationalInvMass(rigidbody.inverseInertiaTensor, rB, normal);
tangentInvMassB += BurstMath.RotationalInvMass(rigidbody.inverseInertiaTensor, rB, tangent);
bitangentInvMassB += BurstMath.RotationalInvMass(rigidbody.inverseInertiaTensor, rB, bitangent);
}
}
public void CalculateContactMassesA(ref NativeArray <float> invMasses,
ref NativeArray <float4> prevPositions,
ref NativeArray <quaternion> orientations,
ref NativeArray <float4> inverseInertiaTensors, bool rollingContacts)
{
// initialize inverse linear masses:
normalInvMassA = tangentInvMassA = bitangentInvMassA = invMasses[entityA];
if (rollingContacts)
{
float4 rA = ContactPointA - prevPositions[entityA];
float4x4 solverInertiaA = BurstMath.TransformInertiaTensor(inverseInertiaTensors[entityA], orientations[entityA]);
normalInvMassA += BurstMath.RotationalInvMass(solverInertiaA, rA, normal);
tangentInvMassA += BurstMath.RotationalInvMass(solverInertiaA, rA, tangent);
bitangentInvMassA += BurstMath.RotationalInvMass(solverInertiaA, rA, bitangent);
}
}
public void CalculateContactMassesA(float invMass,
float4 inverseInertiaTensor,
float4 position,
quaternion orientation,
float4 contactPoint,
bool rollingContacts)
{
// initialize inverse linear masses:
normalInvMassA = tangentInvMassA = bitangentInvMassA = invMass;
if (rollingContacts)
{
float4 rA = contactPoint - position;
float4x4 solverInertiaA = BurstMath.TransformInertiaTensor(inverseInertiaTensor, orientation);
normalInvMassA += BurstMath.RotationalInvMass(solverInertiaA, rA, normal);
tangentInvMassA += BurstMath.RotationalInvMass(solverInertiaA, rA, tangent);
bitangentInvMassA += BurstMath.RotationalInvMass(solverInertiaA, rA, bitangent);
}
}
public void Execute()
{
for (int i = 0; i < activeConstraintCount; ++i)
{
int particleIndex = particleIndices[i];
int colliderIndex = colliderIndices[i];
// no collider to pin to, so ignore the constraint.
if (colliderIndex < 0)
{
continue;
}
int rigidbodyIndex = shapes[colliderIndex].rigidbodyIndex;
// calculate time adjusted compliances
float2 compliances = stiffnesses[i].xy / (deltaTime * deltaTime);
float4 particlePosition = positions[particleIndex];
// express pin offset in world space:
float4 worldPinOffset = transforms[colliderIndex].TransformPoint(offsets[i]);
float4 predictedPinOffset = worldPinOffset;
quaternion predictedRotation = transforms[colliderIndex].rotation;
float rigidbodyLinearW = 0;
float rigidbodyAngularW = 0;
float4 linearRbDelta = float4.zero;
float4 angularRbDelta = float4.zero;
if (rigidbodyIndex >= 0)
{
var rigidbody = rigidbodies[rigidbodyIndex];
linearRbDelta = rigidbodyLinearDeltas[rigidbodyIndex];
angularRbDelta = rigidbodyAngularDeltas[rigidbodyIndex];
// predict world-space position of offset point:
predictedPinOffset = BurstIntegration.IntegrateLinear(predictedPinOffset, rigidbody.GetVelocityAtPoint(worldPinOffset, linearRbDelta, angularRbDelta), deltaTime);
// predict rotation at the end of the step:
predictedRotation = BurstIntegration.IntegrateAngular(predictedRotation, rigidbody.angularVelocity + angularRbDelta, deltaTime);
// calculate linear and angular rigidbody weights:
rigidbodyLinearW = rigidbody.inverseMass;
rigidbodyAngularW = BurstMath.RotationalInvMass(rigidbody.inverseInertiaTensor,
worldPinOffset - rigidbody.com,
math.normalizesafe(inertialFrame.frame.TransformPoint(particlePosition) - predictedPinOffset));
}
// Transform pin position to solver space for constraint solving:
predictedPinOffset = inertialFrame.frame.InverseTransformPoint(predictedPinOffset);
float4 gradient = particlePosition - predictedPinOffset;
float constraint = math.length(gradient);
float4 gradientDir = gradient / (constraint + BurstMath.epsilon);
float4 lambda = lambdas[i];
float linearDLambda = (-constraint - compliances.x * lambda.w) / (invMasses[particleIndex] + rigidbodyLinearW + rigidbodyAngularW + compliances.x + BurstMath.epsilon);
lambda.w += linearDLambda;
float4 correction = linearDLambda * gradientDir;
deltas[particleIndex] += correction * invMasses[particleIndex];
counts[particleIndex]++;
if (rigidbodyAngularW > 0 || invRotationalMasses[particleIndex] > 0)
{
// bend/twist constraint:
quaternion omega = math.mul(math.conjugate(orientations[particleIndex]), predictedRotation); //darboux vector
quaternion omega_plus;
omega_plus.value = omega.value + restDarboux[i].value; //delta Omega with - omega_0
omega.value -= restDarboux[i].value; //delta Omega with + omega_0
if (math.lengthsq(omega.value) > math.lengthsq(omega_plus.value))
{
omega = omega_plus;
}
float3 dlambda = (omega.value.xyz - compliances.y * lambda.xyz) / new float3(compliances.y + invRotationalMasses[particleIndex] + rigidbodyAngularW + BurstMath.epsilon);
lambda.xyz += dlambda;
//discrete Darboux vector does not have vanishing scalar part
quaternion dlambdaQ = new quaternion(dlambda[0], dlambda[1], dlambda[2], 0);
quaternion orientDelta = orientationDeltas[particleIndex];
orientDelta.value += math.mul(predictedRotation, dlambdaQ).value *invRotationalMasses[particleIndex];
orientationDeltas[particleIndex] = orientDelta;
orientationCounts[particleIndex]++;
if (rigidbodyIndex >= 0)
{
rigidbodies[rigidbodyIndex].ApplyDeltaQuaternion(predictedRotation, math.mul(orientations[particleIndex], dlambdaQ).value * -rigidbodyAngularW, ref angularRbDelta, deltaTime);
}
}
if (rigidbodyIndex >= 0)
{
float4 impulse = correction / deltaTime;
rigidbodies[rigidbodyIndex].ApplyImpulse(-inertialFrame.frame.TransformVector(impulse) * 1, worldPinOffset, ref linearRbDelta, ref angularRbDelta);
rigidbodyLinearDeltas[rigidbodyIndex] = linearRbDelta;
rigidbodyAngularDeltas[rigidbodyIndex] = angularRbDelta;
}
lambdas[i] = lambda;
}
}