public void Execute(int workItemIndex)
{
int start, end;
batchData.GetConstraintRange(workItemIndex, out start, out end);
for (int i = start; i < end; ++i)
{
var contact = contacts[i];
// update contact basis:
contact.CalculateBasis(velocities[contact.entityA] - velocities[contact.entityB]);
// update contact masses:
int aMaterialIndex = particleMaterialIndices[contact.entityA];
int bMaterialIndex = particleMaterialIndices[contact.entityB];
bool rollingContacts = (aMaterialIndex >= 0 ? collisionMaterials[aMaterialIndex].rollingContacts > 0 : false) |
(bMaterialIndex >= 0 ? collisionMaterials[bMaterialIndex].rollingContacts > 0 : false);
contact.CalculateContactMassesA(ref invMasses, ref prevPositions, ref prevOrientations, ref invInertiaTensors, rollingContacts);
contact.CalculateContactMassesB(ref invMasses, ref prevPositions, ref prevOrientations, ref invInertiaTensors, rollingContacts);
// update contact distance:
float dAB = math.dot(prevPositions[contact.entityA] - prevPositions[contact.entityB], contact.normal);
float dA = BurstMath.EllipsoidRadius(contact.normal, prevOrientations[contact.entityA], radii[contact.entityA].xyz);
float dB = BurstMath.EllipsoidRadius(contact.normal, prevOrientations[contact.entityB], radii[contact.entityB].xyz);
contact.distance = dAB - (dA + dB);
contacts[i] = contact;
}
}
public void Execute(int p)
{
int i = fluidParticles[p];
if (smoothPositions[i].w > 0)
{
float3 singularValues;
float3x3 u;
BurstMath.EigenSolve(anisotropies[i] / smoothPositions[i].w, out singularValues, out u); //TODO: smoothPositions.w is always 1? we divided it all by w in AverageSmoothPositionsJob...
float max = singularValues[0];
float3 s = math.max(singularValues, new float3(max / maxAnisotropy)) / max * principalRadii[i].x;
principalAxes[i * 3] = new float4(u.c0, s.x);
principalAxes[i * 3 + 1] = new float4(u.c1, s.y);
principalAxes[i * 3 + 2] = new float4(u.c2, s.z);
}
else
{
float radius = principalRadii[i].x / maxAnisotropy;
principalAxes[i * 3] = new float4(1, 0, 0, radius);
principalAxes[i * 3 + 1] = new float4(0, 1, 0, radius);
principalAxes[i * 3 + 2] = new float4(0, 0, 1, radius);
}
renderablePositions[i] = smoothPositions[i];
}
public void Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint)
{
float4 center = shape.center * transform.scale;
point = transform.InverseTransformPointUnscaled(point) - center;
if (shape.is2D != 0)
{
point[2] = 0;
}
int direction = (int)shape.size.z;
float radius = shape.size.x * math.max(transform.scale[(direction + 1) % 3],
transform.scale[(direction + 2) % 3]);
float height = math.max(radius, shape.size.y * 0.5f * transform.scale[direction]);
float4 halfVector = float4.zero;
halfVector[direction] = height - radius;
float4 centerLine = BurstMath.NearestPointOnEdge(-halfVector, halfVector, point, out float mu);
float4 centerToPoint = point - centerLine;
float distanceToCenter = math.length(centerToPoint);
float4 normal = centerToPoint / (distanceToCenter + BurstMath.epsilon);
projectedPoint.point = transform.TransformPointUnscaled(center + centerLine + normal * (radius + shape.contactOffset));
projectedPoint.normal = transform.TransformDirection(normal);
}
public static void Contacts(int particleIndex,
float4 position,
quaternion orientation,
float4 radii,
int colliderIndex,
BurstAffineTransform transform,
BurstColliderShape shape,
NativeQueue <BurstContact> .ParallelWriter contacts)
{
float4 center = shape.center * transform.scale;
position = transform.InverseTransformPointUnscaled(position) - center;
float radius = shape.size.x * math.cmax(transform.scale.xyz);
float distanceToCenter = math.length(position);
float4 normal = position / distanceToCenter;
BurstContact c = new BurstContact
{
entityA = particleIndex,
entityB = colliderIndex,
point = center + normal * radius,
normal = normal,
};
c.point = transform.TransformPointUnscaled(c.point);
c.normal = transform.TransformDirection(c.normal);
c.distance = distanceToCenter - radius - (shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz));
contacts.Enqueue(c);
}
public void Execute(int i)
{
var contact = contacts[i];
int simplexStart = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSize);
// get the material from the first particle in the simplex:
int aMaterialIndex = particleMaterialIndices[simplices[simplexStart]];
bool rollingContacts = aMaterialIndex >= 0 ? collisionMaterials[aMaterialIndex].rollingContacts > 0 : false;
float4 relativeVelocity = float4.zero;
float4 simplexPrevPosition = float4.zero;
quaternion simplexPrevOrientation = new quaternion(0, 0, 0, 0);
float simplexInvMass = 0;
float4 simplexInvInertia = float4.zero;
float simplexRadius = 0;
for (int j = 0; j < simplexSize; ++j)
{
int particleIndex = simplices[simplexStart + j];
relativeVelocity += velocities[particleIndex] * contact.pointA[j];
simplexPrevPosition += prevPositions[particleIndex] * contact.pointA[j];
simplexPrevOrientation.value += prevOrientations[particleIndex].value * contact.pointA[j];
simplexInvMass += invMasses[particleIndex] * contact.pointA[j];
simplexInvInertia += invInertiaTensors[particleIndex] * contact.pointA[j];
simplexRadius += BurstMath.EllipsoidRadius(contact.normal, prevOrientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
}
// if there's a rigidbody present, subtract its velocity from the relative velocity:
int rigidbodyIndex = shapes[contact.bodyB].rigidbodyIndex;
if (rigidbodyIndex >= 0)
{
relativeVelocity -= BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, contact.pointB, rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame);
int bMaterialIndex = shapes[contact.bodyB].materialIndex;
rollingContacts |= bMaterialIndex >= 0 ? collisionMaterials[bMaterialIndex].rollingContacts > 0 : false;
}
// update contact distance
contact.distance = math.dot(simplexPrevPosition - contact.pointB, contact.normal) - simplexRadius;
// calculate contact point in A's surface:
float4 contactPoint = contact.pointB + contact.normal * contact.distance;
// update contact orthonormal basis:
contact.CalculateBasis(relativeVelocity);
// calculate A's contact mass.
contact.CalculateContactMassesA(simplexInvMass, simplexInvInertia, simplexPrevPosition, simplexPrevOrientation, contactPoint, rollingContacts);
// calculate B's contact mass.
if (rigidbodyIndex >= 0)
{
contact.CalculateContactMassesB(rigidbodies[rigidbodyIndex], inertialFrame.frame);
}
contacts[i] = contact;
}
public void Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint)
{
point = transform.InverseTransformPoint(point);
float4 nearestPoint = BurstMath.NearestPointOnTri(tri, point, out float4 bary);
float4 normal = math.normalizesafe(point - nearestPoint);
// flip the contact normal if it points below ground:
BurstMath.OneSidedNormal(triNormal, ref normal);
projectedPoint.point = transform.TransformPoint(nearestPoint + normal * shape.contactOffset);
projectedPoint.normal = transform.TransformDirection(normal);
}
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 Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint)
{
switch (simplexSize)
{
case 1:
{
float4 p1 = positions[simplices[simplexStart]];
projectedPoint.bary = new float4(1, 0, 0, 0);
projectedPoint.point = p1;
}
break;
case 2:
{
float4 p1 = positions[simplices[simplexStart]];
float4 p2 = positions[simplices[simplexStart + 1]];
BurstMath.NearestPointOnEdge(p1, p2, point, out float mu);
projectedPoint.bary = new float4(1 - mu, mu, 0, 0);
projectedPoint.point = p1 * projectedPoint.bary[0] + p2 * projectedPoint.bary[1];
} break;
case 3:
projectedPoint.point = BurstMath.NearestPointOnTri(tri, point, out projectedPoint.bary);
break;
}
projectedPoint.normal = math.normalizesafe(point - projectedPoint.point);
/*float radius1 = radii[simplices[simplexStart]].x;
* float radius2 = radii[simplices[simplexStart+1]].x;
*
* float invLen2 = 1.0f / math.lengthsq(p1 - p2);
* float dl = (radius1 - radius2) * invLen2;
* float sl = math.sqrt(1.0f / invLen2 - math.pow(radius1 - radius2, 2)) * math.sqrt(invLen2);
* float adj_radii1 = radius1 * sl;
* float adj_radii2 = radius2 * sl;
*
* float trange1 = radius1 * dl;
* float trange2 = 1 + radius2 * dl;
*
* float adj_t = (mu - trange1) / (trange2 - trange1);
* float radius = adj_radii1 + adj_t * (adj_radii2 - adj_radii1);
*
* float4 centerToPoint = point - centerLine;
* float4 normal = centerToPoint / (math.length(centerToPoint) + BurstMath.epsilon);
*
* projectedPoint.point = centerLine + normal * radius;
* projectedPoint.normal = normal;*/
}
public void Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint)
{
point = transform.InverseTransformPointUnscaled(point);
if (shape.is2D != 0)
{
point[2] = 0;
}
float4 nearestPoint = BurstMath.NearestPointOnTri(tri, point, out float4 bary);
float4 normal = math.normalizesafe(point - nearestPoint);
projectedPoint.point = transform.TransformPointUnscaled(nearestPoint + normal * shape.contactOffset);
projectedPoint.normal = transform.TransformDirection(normal);
}
private float4 GetRelativeVelocity(int particleIndex, int rigidbodyIndex, ref BurstContact contact)
{
// Initialize with particle linear velocity:
float4 relativeVelocity = (positions[particleIndex] - prevPositions[particleIndex]) / dt;
// As we do not consider true ellipses for collision detection, particle contact points are never off-axis.
// So particle angular velocity does not contribute to normal impulses, and we can skip it.
// Subtract rigidbody velocity:
if (rigidbodyIndex >= 0)
{
relativeVelocity -= BurstMath.GetRigidbodyVelocityAtPoint(rigidbodies[rigidbodyIndex], contact.ContactPointB, rigidbodyLinearDeltas[rigidbodyIndex], rigidbodyAngularDeltas[rigidbodyIndex], inertialFrame.frame);
}
return(relativeVelocity);
}
public void Execute(int workItemIndex)
{
int start, end;
batchData.GetConstraintRange(workItemIndex, out start, out end);
for (int i = start; i < end; ++i)
{
var pair = pairs[i];
float4 distanceA = renderablePositions[pair.particleB] - smoothPositions[pair.particleA];
float4 distanceB = renderablePositions[pair.particleA] - smoothPositions[pair.particleB];
anisotropies[pair.particleA] += BurstMath.multrnsp(distanceA, distanceA) * pair.avgKernel;
anisotropies[pair.particleB] += BurstMath.multrnsp(distanceB, distanceB) * pair.avgKernel;
}
}
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 Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint)
{
point = transform.InverseTransformPointUnscaled(point);
if (shape.is2D != 0)
{
point[2] = 0;
}
Edge t = edges[header.firstEdge + dataOffset];
float4 v1 = (new float4(vertices[header.firstVertex + t.i1], 0) + shape.center) * transform.scale;
float4 v2 = (new float4(vertices[header.firstVertex + t.i2], 0) + shape.center) * transform.scale;
float4 nearestPoint = BurstMath.NearestPointOnEdge(v1, v2, point, out float mu);
float4 normal = math.normalizesafe(point - nearestPoint);
projectedPoint.normal = transform.TransformDirection(normal);
projectedPoint.point = transform.TransformPointUnscaled(nearestPoint + normal * shape.contactOffset);
}
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(int i)
{
var contact = contacts[i];
int aMaterialIndex = particleMaterialIndices[contact.entityA];
bool rollingContacts = aMaterialIndex >= 0 ? collisionMaterials[aMaterialIndex].rollingContacts > 0 : false;
int rigidbodyIndex = shapes[contact.entityB].rigidbodyIndex;
if (rigidbodyIndex >= 0)
{
// update contact basis:
float4 relativeVelocity = velocities[contact.entityA] - BurstMath.GetRigidbodyVelocityAtPoint(rigidbodies[rigidbodyIndex], contact.ContactPointB, rigidbodyLinearDeltas[rigidbodyIndex], rigidbodyAngularDeltas[rigidbodyIndex], inertialFrame.frame);
contact.CalculateBasis(relativeVelocity);
int bMaterialIndex = shapes[contact.entityB].materialIndex;
rollingContacts |= bMaterialIndex >= 0 ? collisionMaterials[bMaterialIndex].rollingContacts > 0 : false;
// update contact masses:
contact.CalculateContactMassesA(ref invMasses, ref prevPositions, ref prevOrientations, ref invInertiaTensors, rollingContacts);
contact.CalculateContactMassesB(rigidbodies[rigidbodyIndex], false);
}
else
{
// update contact basis:
contact.CalculateBasis(velocities[contact.entityA]);
// update contact masses:
contact.CalculateContactMassesA(ref invMasses, ref prevPositions, ref prevOrientations, ref invInertiaTensors, rollingContacts);
}
// update contact distance
float dAB = math.dot(prevPositions[contact.entityA] - contact.point, contact.normal);
float dA = BurstMath.EllipsoidRadius(contact.normal, prevOrientations[contact.entityA], radii[contact.entityA].xyz);
float dB = shapes[contact.entityB].contactOffset;
contact.distance = dAB - (dA + dB);
contacts[i] = contact;
}
private float4 GetRelativeVelocity(int particleIndex, int rigidbodyIndex, ref BurstContact contact, ref float4 angularVelocityA, ref float4 rA, ref float4 rB, bool rollingContacts)
{
// Initialize with particle linear velocity:
float4 relativeVelocity = (positions[particleIndex] - prevPositions[particleIndex]) / dt;
// Add particle angular velocity if rolling contacts are enabled:
if (rollingContacts)
{
angularVelocityA = BurstIntegration.DifferentiateAngular(orientations[particleIndex], prevOrientations[particleIndex], dt);
rA = contact.ContactPointA - prevPositions[particleIndex];
relativeVelocity += new float4(math.cross(angularVelocityA.xyz, rA.xyz), 0);
}
// Subtract rigidbody velocity:
if (rigidbodyIndex >= 0)
{
// Note: unlike rA, that is expressed in solver space, rB is expressed in world space.
rB = inertialFrame.frame.TransformPoint(contact.ContactPointB) - rigidbodies[rigidbodyIndex].com;
relativeVelocity -= BurstMath.GetRigidbodyVelocityAtPoint(rigidbodies[rigidbodyIndex], contact.ContactPointB, rigidbodyLinearDeltas[rigidbodyIndex], rigidbodyAngularDeltas[rigidbodyIndex], inertialFrame.frame);
}
return(relativeVelocity);
}
// The code actually running on the job
public void Execute(int index)
{
int i = activeParticles[index];
// Project particles on the XY plane if we are in 2D mode:
if (is2D)
{
// restrict position to the 2D plane
float4 pos = positions[i];
pos[2] = previousPositions[i][2];
positions[i] = pos;
// restrict rotation to the axis perpendicular to the 2D plane.
quaternion swing, twist;
BurstMath.SwingTwist(orientations[i], new float3(0, 0, 1), out swing, out twist);
orientations[i] = twist;
}
if (inverseMasses[i] > 0)
{
velocities[i] = BurstIntegration.DifferentiateLinear(positions[i], previousPositions[i], deltaTime);
}
else
{
velocities[i] = float4.zero;
}
if (inverseRotationalMasses[i] > 0)
{
angularVelocities[i] = BurstIntegration.DifferentiateAngular(orientations[i], previousOrientations[i], deltaTime);
}
else
{
angularVelocities[i] = float4.zero;
}
}
public void Execute(int i)
{
int k = 0;
float maximumMass = 10000;
coms[i] = float4.zero;
float4x4 Apq = float4x4.zero, Rpq = float4x4.zero;
// calculate shape mass, center of mass, and moment matrix:
for (int j = 0; j < numIndices[i]; ++j)
{
k = particleIndices[firstIndex[i] + j];
float mass = maximumMass;
if (invMasses[k] > 1.0f / maximumMass)
{
mass = 1.0f / invMasses[k];
}
coms[i] += positions[k] * mass;
float4x4 particleR = orientations[k].toMatrix();
float4x4 particleRT = restOrientations[k].toMatrix();
particleR[3][3] = 0;
particleRT[3][3] = 0;
Rpq += math.mul(particleR,
math.mul(math.rcp(invInertiaTensors[k] + new float4(BurstMath.epsilon)).asDiagonal(),
math.transpose(particleRT))
);
float4 restPosition = restPositions[k];
restPosition[3] = 0;
Apq += mass * BurstMath.multrnsp4(positions[k], restPosition);
}
if (restComs[i][3] < BurstMath.epsilon)
{
return;
}
coms[i] /= restComs[i][3];
// subtract global shape moment:
float4 restCom = restComs[i];
restCom[3] = 0;
Apq -= restComs[i][3] * BurstMath.multrnsp4(coms[i], restCom);
// calculate optimal transform including plastic deformation:
float4x4 Apq_def = Rpq + math.mul(Apq, math.transpose(deformation[i]));
// extract rotation from transform matrix, using warmstarting and few iterations:
constraintOrientations[i] = BurstMath.ExtractRotation(Apq_def, constraintOrientations[i], 2);
// finally, obtain rotation matrix:
float4x4 R = constraintOrientations[i].toMatrix();
R[3][3] = 0;
// calculate particle orientations:
if (explicitGroup[i] > 0)
{
// if the group is explicit, set the orientation for all particles:
for (int j = 0; j < numIndices[i]; ++j)
{
k = particleIndices[firstIndex[i] + j];
orientations[k] = math.mul(constraintOrientations[i], restOrientations[k]);
}
}
else
{
// set orientation of center particle only:
int centerIndex = particleIndices[firstIndex[i]];
orientations[centerIndex] = math.mul(constraintOrientations[i], restOrientations[centerIndex]);
}
// calculate and accumulate particle goal positions:
float4 goal;
float4x4 transform = math.mul(R, deformation[i]);
for (int j = 0; j < numIndices[i]; ++j)
{
k = particleIndices[firstIndex[i] + j];
goal = coms[i] + math.mul(transform, restPositions[k] - restComs[i]);
deltas[k] += (goal - positions[k]) * shapeMaterialParameters[i * 5];
counts[k]++;
}
// update plastic deformation:
float plastic_yield = shapeMaterialParameters[i * 5 + 1];
float plastic_creep = shapeMaterialParameters[i * 5 + 2];
float plastic_recovery = shapeMaterialParameters[i * 5 + 3];
float max_deform = shapeMaterialParameters[i * 5 + 4];
// if we are allowed to absorb deformation:
if (plastic_creep > 0)
{
R[3][3] = 1;
Apq_def[3][3] = 1;
// get scale matrix (A = RS so S = Rt * A) and its deviation from the identity matrix:
float4x4 deform_matrix = math.mul(math.transpose(R), math.mul(Apq_def, Aqq[i])) - float4x4.identity;
// if the amount of deformation exceeds the yield threshold:
float norm = deform_matrix.frobeniusNorm();
if (norm > plastic_yield)
{
// deform the shape permanently:
deformation[i] = math.mul(float4x4.identity + plastic_creep * deform_matrix, deformation[i]);
// clamp deformation so that it does not exceed a percentage;
deform_matrix = deformation[i] - float4x4.identity;
norm = deform_matrix.frobeniusNorm();
if (norm > max_deform)
{
deformation[i] = float4x4.identity + max_deform * deform_matrix / norm;
}
// if we cannot recover from plastic deformation, recalculate rest shape now:
if (plastic_recovery == 0)
{
RecalculateRestData(i,
ref particleIndices,
ref firstIndex,
ref restComs,
ref Aqq,
ref deformation,
ref numIndices,
ref invMasses,
ref restPositions,
ref restOrientations,
ref invInertiaTensors);
}
}
}
// if we can recover from plastic deformation, lerp towards non-deformed shape and recalculate rest shape:
if (plastic_recovery > 0)
{
deformation[i] += (float4x4.identity - deformation[i]) * math.min(plastic_recovery * deltaTime, 1.0f);
RecalculateRestData(i,
ref particleIndices,
ref firstIndex,
ref restComs,
ref Aqq,
ref deformation,
ref numIndices,
ref invMasses,
ref restPositions,
ref restOrientations,
ref invInertiaTensors);
}
}
protected static void RecalculateRestData(int i,
ref NativeArray <int> particleIndices,
ref NativeArray <int> firstIndex,
ref NativeArray <float4> restComs,
ref NativeArray <float4x4> Aqq,
ref NativeArray <float4x4> deformation,
ref NativeArray <int> numIndices,
ref NativeArray <float> invMasses,
ref NativeArray <float4> restPositions,
ref NativeArray <quaternion> restOrientations,
ref NativeArray <float4> invInertiaTensors)
{
int k = 0;
float maximumMass = 10000;
// initialize rest center of mass and shape matrix:
restComs[i] = float4.zero;
Aqq[i] = float4x4.zero;
float4 restCom = float4.zero;
float4x4 _Aqq = float4x4.zero, _Rqq = float4x4.zero;
// calculate rest center of mass, shape mass and Aqq matrix.
for (int j = 0; j < numIndices[i]; ++j)
{
k = particleIndices[firstIndex[i] + j];
float mass = maximumMass;
if (invMasses[k] > 1.0f / maximumMass)
{
mass = 1.0f / invMasses[k];
}
restCom += restPositions[k] * mass;
float4x4 particleR = restOrientations[k].toMatrix();
particleR[3][3] = 0;
_Rqq += math.mul(particleR,
math.mul(math.rcp(invInertiaTensors[k] + new float4(BurstMath.epsilon)).asDiagonal(),
math.transpose(particleR))
);
float4 restPosition = restPositions[k];
restPosition[3] = 0;
_Aqq += mass * BurstMath.multrnsp4(restPosition, restPosition);
}
if (restCom[3] < BurstMath.epsilon)
{
return;
}
restCom.xyz /= restCom[3];
restComs[i] = restCom;
restCom[3] = 0;
_Aqq -= restComs[i][3] * BurstMath.multrnsp4(restCom, restCom);
_Aqq[3][3] = 1; // so that the determinant is never 0 due to all-zeros row/column.
Aqq[i] = math.inverse(_Rqq + math.mul(deformation[i], math.mul(_Aqq, math.transpose(deformation[i]))));
}
private void InteractionTest(int A, int B, ref BurstSimplex simplexShape)
{
// skip the pair if their bounds don't intersect:
if (!simplexBounds[A].IntersectsAabb(simplexBounds[B]))
{
return;
}
// get the start index and size of each simplex:
int simplexStartA = simplexCounts.GetSimplexStartAndSize(A, out int simplexSizeA);
int simplexStartB = simplexCounts.GetSimplexStartAndSize(B, out int simplexSizeB);
// immediately reject simplex pairs that share particles:
for (int a = 0; a < simplexSizeA; ++a)
{
for (int b = 0; b < simplexSizeB; ++b)
{
if (simplices[simplexStartA + a] == simplices[simplexStartB + b])
{
return;
}
}
}
// get phases for each simplex:
bool restPositionsEnabled = false;
int groupA = GetSimplexPhase(simplexStartA, simplexSizeA, out Oni.ParticleFlags flagsA, ref restPositionsEnabled);
int groupB = GetSimplexPhase(simplexStartB, simplexSizeB, out Oni.ParticleFlags flagsB, ref restPositionsEnabled);
// if all particles have the same group and none have self-collision, reject the pair.
if (groupA == groupB && (flagsA & flagsB & Oni.ParticleFlags.SelfCollide) == 0)
{
return;
}
// if all simplices are fluid, check their smoothing radii:
if ((flagsA & Oni.ParticleFlags.Fluid) != 0 && (flagsB & Oni.ParticleFlags.Fluid) != 0)
{
int particleA = simplices[simplexStartA];
int particleB = simplices[simplexStartB];
// for fluid we only consider the first particle in each simplex.
float4 predictedPositionA = positions[particleA] + velocities[particleA] * dt;
float4 predictedPositionB = positions[particleB] + velocities[particleB] * dt;
// Calculate particle center distance:
float d2 = math.lengthsq(predictedPositionA - predictedPositionB);
float fluidDistance = math.max(fluidRadii[particleA], fluidRadii[particleB]);
if (d2 <= fluidDistance * fluidDistance)
{
fluidInteractionsQueue.Enqueue(new FluidInteraction {
particleA = particleA, particleB = particleB
});
}
}
else // at least one solid particle is present:
{
// swap simplices so that B is always the one-sided one.
if ((flagsA & Oni.ParticleFlags.OneSided) != 0 && groupA < groupB)
{
ObiUtils.Swap(ref A, ref B);
ObiUtils.Swap(ref simplexStartA, ref simplexStartB);
ObiUtils.Swap(ref simplexSizeA, ref simplexSizeB);
ObiUtils.Swap(ref flagsA, ref flagsB);
ObiUtils.Swap(ref groupA, ref groupB);
}
float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSizeA);
float4 simplexPoint;
simplexShape.simplexStart = simplexStartB;
simplexShape.simplexSize = simplexSizeB;
simplexShape.positions = restPositions;
simplexShape.CacheData();
float simplexRadiusA = 0, simplexRadiusB = 0;
// skip the contact if there's self-intersection at rest:
if (groupA == groupB && restPositionsEnabled)
{
var restPoint = BurstLocalOptimization.Optimize <BurstSimplex>(ref simplexShape, restPositions, radii,
simplices, simplexStartA, simplexSizeA, ref simplexBary, out simplexPoint, 4, 0);
for (int j = 0; j < simplexSizeA; ++j)
{
simplexRadiusA += radii[simplices[simplexStartA + j]].x * simplexBary[j];
}
for (int j = 0; j < simplexSizeB; ++j)
{
simplexRadiusB += radii[simplices[simplexStartB + j]].x * restPoint.bary[j];
}
// compare distance along contact normal with radius.
if (math.dot(simplexPoint - restPoint.point, restPoint.normal) < simplexRadiusA + simplexRadiusB)
{
return;
}
}
simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSizeA);
simplexShape.positions = positions;
simplexShape.CacheData();
var surfacePoint = BurstLocalOptimization.Optimize <BurstSimplex>(ref simplexShape, positions, radii,
simplices, simplexStartA, simplexSizeA, ref simplexBary, out simplexPoint, optimizationIterations, optimizationTolerance);
simplexRadiusA = 0; simplexRadiusB = 0;
float4 velocityA = float4.zero, velocityB = float4.zero, normalB = float4.zero;
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
simplexRadiusA += radii[particleIndex].x * simplexBary[j];
velocityA += velocities[particleIndex] * simplexBary[j];
}
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
simplexRadiusB += radii[particleIndex].x * surfacePoint.bary[j];
velocityB += velocities[particleIndex] * surfacePoint.bary[j];
normalB += normals[particleIndex] * surfacePoint.bary[j];
}
float dAB = math.dot(simplexPoint - surfacePoint.point, surfacePoint.normal);
float vel = math.dot(velocityA - velocityB, surfacePoint.normal);
// check if the projected velocity along the contact normal will get us within collision distance.
if (vel * dt + dAB <= simplexRadiusA + simplexRadiusB + collisionMargin)
{
// adapt collision normal for one-sided simplices:
if ((flagsB & Oni.ParticleFlags.OneSided) != 0 && groupB < groupA)
{
BurstMath.OneSidedNormal(normalB, ref surfacePoint.normal);
}
contactsQueue.Enqueue(new BurstContact()
{
bodyA = A,
bodyB = B,
pointA = simplexBary,
pointB = surfacePoint.bary,
normal = surfacePoint.normal
});
}
}
}
public static void Contacts(int particleIndex,
int colliderIndex,
float4 position,
quaternion orientation,
float4 radii,
ref NativeArray <float> heightMap,
HeightFieldHeader header,
BurstAffineTransform colliderToSolver,
BurstColliderShape shape,
NativeQueue <BurstContact> .ParallelWriter contacts)
{
float4 pos = colliderToSolver.InverseTransformPoint(position);
BurstContact c = new BurstContact
{
entityA = particleIndex,
entityB = colliderIndex,
};
int resolutionU = (int)shape.center.x;
int resolutionV = (int)shape.center.y;
// calculate terrain cell size:
float cellWidth = shape.size.x / (resolutionU - 1);
float cellHeight = shape.size.z / (resolutionV - 1);
// calculate particle bounds min/max cells:
int2 min = new int2((int)math.floor((pos[0] - radii[0]) / cellWidth), (int)math.floor((pos[2] - radii[0]) / cellHeight));
int2 max = new int2((int)math.floor((pos[0] + radii[0]) / cellWidth), (int)math.floor((pos[2] + radii[0]) / cellHeight));
for (int su = min[0]; su <= max[0]; ++su)
{
if (su >= 0 && su < resolutionU - 1)
{
for (int sv = min[1]; sv <= max[1]; ++sv)
{
if (sv >= 0 && sv < resolutionV - 1)
{
// calculate neighbor sample indices:
int csu1 = math.clamp(su + 1, 0, resolutionU - 1);
int csv1 = math.clamp(sv + 1, 0, resolutionV - 1);
// sample heights:
float h1 = heightMap[header.firstSample + sv * resolutionU + su] * shape.size.y;
float h2 = heightMap[header.firstSample + sv * resolutionU + csu1] * shape.size.y;
float h3 = heightMap[header.firstSample + csv1 * resolutionU + su] * shape.size.y;
float h4 = heightMap[header.firstSample + csv1 * resolutionU + csu1] * shape.size.y;
float min_x = su * shape.size.x / (resolutionU - 1);
float max_x = csu1 * shape.size.x / (resolutionU - 1);
float min_z = sv * shape.size.z / (resolutionV - 1);
float max_z = csv1 * shape.size.z / (resolutionV - 1);
// contact with the first triangle:
float4 pointOnTri = BurstMath.NearestPointOnTri(new float4(min_x, h3, max_z, 0),
new float4(max_x, h4, max_z, 0),
new float4(min_x, h1, min_z, 0),
pos);
float4 normal = pos - pointOnTri;
float distance = math.length(normal);
if (distance > BurstMath.epsilon)
{
c.normal = normal / distance;
c.point = pointOnTri;
c.normal = colliderToSolver.TransformDirection(c.normal);
c.point = colliderToSolver.TransformPoint(c.point);
c.distance = distance - (shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz));
contacts.Enqueue(c);
}
// contact with the second triangle:
pointOnTri = BurstMath.NearestPointOnTri(new float4(min_x, h1, min_z, 0),
new float4(max_x, h4, max_z, 0),
new float4(max_x, h2, min_z, 0),
pos);
normal = pos - pointOnTri;
distance = math.length(normal);
if (distance > BurstMath.epsilon)
{
c.normal = normal / distance;
c.point = pointOnTri;
c.normal = colliderToSolver.TransformDirection(c.normal);
c.point = colliderToSolver.TransformPoint(c.point);
c.distance = distance - (shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz));
contacts.Enqueue(c);
}
}
}
}
}
}
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;
}
}
public static void Contacts(int particleIndex,
float4 position,
quaternion orientation,
float4 radii,
int colliderIndex,
BurstAffineTransform transform,
BurstColliderShape shape,
NativeQueue <BurstContact> .ParallelWriter contacts)
{
BurstContact c = new BurstContact()
{
entityA = particleIndex,
entityB = colliderIndex,
};
float4 center = shape.center * transform.scale;
position = transform.InverseTransformPointUnscaled(position) - center;
int direction = (int)shape.size.z;
float radius = shape.size.x * math.max(transform.scale[(direction + 1) % 3], transform.scale[(direction + 2) % 3]);
float height = math.max(radius, shape.size.y * 0.5f * transform.scale[direction]);
float d = position[direction];
float4 axisProj = float4.zero;
float4 cap = float4.zero;
axisProj[direction] = d;
cap[direction] = height - radius;
float4 centerToPoint;
float centerToPointNorm;
if (d > height - radius)
{ //one cap
centerToPoint = position - cap;
centerToPointNorm = math.length(centerToPoint);
c.distance = centerToPointNorm - radius;
c.normal = (centerToPoint / (centerToPointNorm + math.FLT_MIN_NORMAL));
c.point = cap + c.normal * radius;
}
else if (d < -height + radius)
{ // other cap
centerToPoint = position + cap;
centerToPointNorm = math.length(centerToPoint);
c.distance = centerToPointNorm - radius;
c.normal = (centerToPoint / (centerToPointNorm + math.FLT_MIN_NORMAL));
c.point = -cap + c.normal * radius;
}
else
{//cylinder
centerToPoint = position - axisProj;
centerToPointNorm = math.length(centerToPoint);
c.distance = centerToPointNorm - radius;
c.normal = (centerToPoint / (centerToPointNorm + math.FLT_MIN_NORMAL));
c.point = axisProj + c.normal * radius;
}
c.point += center;
c.point = transform.TransformPointUnscaled(c.point);
c.normal = transform.TransformDirection(c.normal);
c.distance -= shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz);
contacts.Enqueue(c);
}
public void InteractionTest(int A, int B)
{
bool samePhase = (phases[A] & (int)Oni.ParticleFlags.GroupMask) == (phases[B] & (int)Oni.ParticleFlags.GroupMask);
bool noSelfCollide = (phases[A] & (int)Oni.ParticleFlags.SelfCollide) == 0 || (phases[B] & (int)Oni.ParticleFlags.SelfCollide) == 0;
// only particles of a different phase or set to self-collide can interact.
if (samePhase && noSelfCollide)
{
return;
}
// Predict positions at the end of the whole step:
float4 predictedPositionA = positions[A] + velocities[A] * dt;
float4 predictedPositionB = positions[B] + velocities[B] * dt;
// Calculate particle center distance:
float4 dab = predictedPositionA - predictedPositionB;
float d2 = math.lengthsq(dab);
// if both particles are fluid, check their smoothing radii:
if ((phases[A] & (int)Oni.ParticleFlags.Fluid) != 0 &&
(phases[B] & (int)Oni.ParticleFlags.Fluid) != 0)
{
float fluidDistance = math.max(fluidRadii[A], fluidRadii[B]);
if (d2 <= fluidDistance * fluidDistance)
{
fluidInteractionsQueue.Enqueue(new FluidInteraction {
particleA = A, particleB = B
});
}
}
else // at least one solid particle
{
float solidDistance = radii[A].x + radii[B].x;
// if these particles are self-colliding (have same phase), see if they intersect at rest.
if (samePhase &&
restPositions[A].w > 0.5f &&
restPositions[B].w > 0.5f)
{
// if rest positions intersect, return too.
float sqr_rest_distance = math.lengthsq(restPositions[A] - restPositions[B]);
if (sqr_rest_distance < solidDistance * solidDistance)
{
return;
}
}
// calculate distance at which particles are able to interact:
int matIndexA = particleMaterialIndices[A];
int matIndexB = particleMaterialIndices[B];
float interactionDistance = solidDistance * 1.2f + (matIndexA >= 0 ? collisionMaterials[matIndexA].stickDistance : 0) +
(matIndexB >= 0 ? collisionMaterials[matIndexB].stickDistance : 0);
// if the distance between their predicted positions is smaller than the interaction distance:
if (math.lengthsq(dab) <= interactionDistance * interactionDistance)
{
// calculate contact normal and distance:
float4 normal = positions[A] - positions[B];
float distance = math.length(normal);
if (distance > BurstMath.epsilon)
{
normal /= distance;
float rA = BurstMath.EllipsoidRadius(normal, orientations[A], radii[A].xyz);
float rB = BurstMath.EllipsoidRadius(normal, orientations[B], radii[B].xyz);
// adapt normal for one-sided particles:
if ((phases[A] & (int)Oni.ParticleFlags.OneSided) != 0 &&
(phases[B] & (int)Oni.ParticleFlags.OneSided) != 0)
{
float3 adjustment = float3.zero;
if (rA < rB)
{
adjustment = math.mul(orientations[A], new float3(0, 0, -1));
}
else
{
adjustment = math.mul(orientations[B], new float3(0, 0, 1));
}
float dot = math.dot(normal.xyz, adjustment);
if (dot < 0)
{
normal -= 2 * dot * new float4(adjustment, 0);
}
}
contactsQueue.Enqueue(new BurstContact
{
entityA = A,
entityB = B,
point = positions[B] + normal * rB,
normal = normal,
distance = distance - (rA + rB)
});
}
}
}
}
public void Execute(int workItemIndex)
{
int start, end;
batchData.GetConstraintRange(workItemIndex, out start, out end);
for (int i = start; i < end; ++i)
{
var contact = contacts[i];
int simplexStartA = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSizeA);
int simplexStartB = simplexCounts.GetSimplexStartAndSize(contact.bodyB, out int simplexSizeB);
float4 simplexVelocityA = float4.zero;
float4 simplexPrevPositionA = float4.zero;
quaternion simplexPrevOrientationA = new quaternion(0, 0, 0, 0);
float simplexRadiusA = 0;
float simplexInvMassA = 0;
float4 simplexInvInertiaA = float4.zero;
float4 simplexVelocityB = float4.zero;
float4 simplexPrevPositionB = float4.zero;
quaternion simplexPrevOrientationB = new quaternion(0, 0, 0, 0);
float simplexRadiusB = 0;
float simplexInvMassB = 0;
float4 simplexInvInertiaB = float4.zero;
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
simplexVelocityA += velocities[particleIndex] * contact.pointA[j];
simplexPrevPositionA += prevPositions[particleIndex] * contact.pointA[j];
simplexPrevOrientationA.value += prevOrientations[particleIndex].value * contact.pointA[j];
simplexInvMassA += invMasses[particleIndex] * contact.pointA[j];
simplexInvInertiaA += invInertiaTensors[particleIndex] * contact.pointA[j];
simplexRadiusA += BurstMath.EllipsoidRadius(contact.normal, prevOrientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
}
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
simplexVelocityB += velocities[particleIndex] * contact.pointB[j];
simplexPrevPositionB += prevPositions[particleIndex] * contact.pointB[j];
simplexPrevOrientationB.value += prevOrientations[particleIndex].value * contact.pointB[j];
simplexInvMassB += invMasses[particleIndex] * contact.pointB[j];
simplexInvInertiaB += invInertiaTensors[particleIndex] * contact.pointB[j];
simplexRadiusB += BurstMath.EllipsoidRadius(contact.normal, prevOrientations[particleIndex], radii[particleIndex].xyz) * contact.pointB[j];
}
// update contact distance
float dAB = math.dot(simplexPrevPositionA - simplexPrevPositionB, contact.normal);
contact.distance = dAB - (simplexRadiusA + simplexRadiusB);
// calculate contact points:
float4 contactPointA = simplexPrevPositionB + contact.normal * (contact.distance + simplexRadiusB);
float4 contactPointB = simplexPrevPositionA - contact.normal * (contact.distance + simplexRadiusA);
// update contact basis:
contact.CalculateBasis(simplexVelocityA - simplexVelocityB);
// update contact masses:
int aMaterialIndex = particleMaterialIndices[simplices[simplexStartA]];
int bMaterialIndex = particleMaterialIndices[simplices[simplexStartB]];
bool rollingContacts = (aMaterialIndex >= 0 ? collisionMaterials[aMaterialIndex].rollingContacts > 0 : false) |
(bMaterialIndex >= 0 ? collisionMaterials[bMaterialIndex].rollingContacts > 0 : false);
contact.CalculateContactMassesA(simplexInvMassA, simplexInvInertiaA, simplexPrevPositionA, simplexPrevOrientationA, contactPointA, rollingContacts);
contact.CalculateContactMassesB(simplexInvMassB, simplexInvInertiaB, simplexPrevPositionB, simplexPrevOrientationB, contactPointB, rollingContacts);
contacts[i] = contact;
}
}
public void Execute(int workItemIndex)
{
int start, end;
batchData.GetConstraintRange(workItemIndex, out start, out end);
for (int i = start; i < end; ++i)
{
var contact = contacts[i];
int indexA = contact.entityA;
int indexB = contact.entityB;
// Combine collision materials:
BurstCollisionMaterial material = CombineCollisionMaterials(contact.entityA, contact.entityB);
// Calculate relative velocity:
float4 angularVelocityA = float4.zero, angularVelocityB = float4.zero, rA = float4.zero, rB = float4.zero;
float4 relativeVelocity = GetRelativeVelocity(indexA, indexB, ref contact, ref angularVelocityA, ref angularVelocityB, ref rA, ref rB, material.rollingContacts > 0);
// Calculate friction impulses (in the tangent and bitangent ddirections):
float2 impulses = contact.SolveFriction(relativeVelocity, material.staticFriction, material.dynamicFriction, dt);
// Apply friction impulses to both particles:
if (math.abs(impulses.x) > BurstMath.epsilon || math.abs(impulses.y) > BurstMath.epsilon)
{
float4 tangentImpulse = impulses.x * contact.tangent;
float4 bitangentImpulse = impulses.y * contact.bitangent;
float4 totalImpulse = tangentImpulse + bitangentImpulse;
deltas[indexA] += (tangentImpulse * contact.tangentInvMassA + bitangentImpulse * contact.bitangentInvMassA) * dt;
deltas[indexB] -= (tangentImpulse * contact.tangentInvMassB + bitangentImpulse * contact.bitangentInvMassB) * dt;
counts[indexA]++;
counts[indexB]++;
// Rolling contacts:
if (material.rollingContacts > 0)
{
// Calculate angular velocity deltas due to friction impulse:
float4x4 solverInertiaA = BurstMath.TransformInertiaTensor(invInertiaTensors[indexA], orientations[indexA]);
float4x4 solverInertiaB = BurstMath.TransformInertiaTensor(invInertiaTensors[indexB], orientations[indexB]);
float4 angVelDeltaA = math.mul(solverInertiaA, new float4(math.cross(rA.xyz, totalImpulse.xyz), 0));
float4 angVelDeltaB = -math.mul(solverInertiaB, new float4(math.cross(rB.xyz, totalImpulse.xyz), 0));
// Final angular velocities, after adding the deltas:
angularVelocityA += angVelDeltaA;
angularVelocityB += angVelDeltaB;
// Calculate weights (inverse masses):
float invMassA = math.length(math.mul(solverInertiaA, math.normalizesafe(angularVelocityA)));
float invMassB = math.length(math.mul(solverInertiaB, math.normalizesafe(angularVelocityB)));
// Calculate rolling axis and angular velocity deltas:
float4 rollAxis = float4.zero;
float rollingImpulse = contact.SolveRollingFriction(angularVelocityA, angularVelocityB, material.rollingFriction, invMassA, invMassB, ref rollAxis);
angVelDeltaA += rollAxis * rollingImpulse * invMassA;
angVelDeltaB -= rollAxis * rollingImpulse * invMassB;
// Apply orientation deltas to particles:
quaternion orientationDeltaA = BurstIntegration.AngularVelocityToSpinQuaternion(orientations[indexA], angVelDeltaA);
quaternion orientationDeltaB = BurstIntegration.AngularVelocityToSpinQuaternion(orientations[indexB], angVelDeltaB);
quaternion qA = orientationDeltas[indexA];
qA.value += orientationDeltaA.value * dt;
orientationDeltas[indexA] = qA;
orientationCounts[indexA]++;
quaternion qB = orientationDeltas[indexB];
qB.value += orientationDeltaB.value * dt;
orientationDeltas[indexB] = qB;
orientationCounts[indexB]++;
}
}
contacts[i] = contact;
}
}
public void Execute(int workItemIndex)
{
int start, end;
batchData.GetConstraintRange(workItemIndex, out start, out end);
for (int i = start; i < end; ++i)
{
var contact = contacts[i];
int simplexStartA = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSizeA);
int simplexStartB = simplexCounts.GetSimplexStartAndSize(contact.bodyB, out int simplexSizeB);
// Combine collision materials:
BurstCollisionMaterial material = CombineCollisionMaterials(simplices[simplexStartA], simplices[simplexStartB]);
float4 simplexPositionA = float4.zero, simplexPositionB = float4.zero;
float simplexRadiusA = 0, simplexRadiusB = 0;
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
simplexPositionA += positions[particleIndex] * contact.pointA[j];
simplexRadiusA += BurstMath.EllipsoidRadius(contact.normal, orientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
}
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
simplexPositionB += positions[particleIndex] * contact.pointB[j];
simplexRadiusB += BurstMath.EllipsoidRadius(contact.normal, orientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
}
float4 posA = simplexPositionA - contact.normal * simplexRadiusA;
float4 posB = simplexPositionB + contact.normal * simplexRadiusB;
// adhesion:
float lambda = contact.SolveAdhesion(posA, posB, material.stickDistance, material.stickiness, substepTime);
// depenetration:
lambda += contact.SolvePenetration(posA, posB, solverParameters.maxDepenetration * substepTime);
// Apply normal impulse to both particles (w/ shock propagation):
if (math.abs(lambda) > BurstMath.epsilon)
{
float shock = solverParameters.shockPropagation * math.dot(contact.normal, math.normalizesafe(gravity));
float4 delta = lambda * contact.normal;
float baryScale = BurstMath.BaryScale(contact.pointA);
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
deltas[particleIndex] += delta * invMasses[particleIndex] * contact.pointA[j] * baryScale * (1 - shock);
counts[particleIndex]++;
}
baryScale = BurstMath.BaryScale(contact.pointB);
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
deltas[particleIndex] -= delta * invMasses[particleIndex] * contact.pointB[j] * baryScale * (1 + shock);
counts[particleIndex]++;
}
}
// Apply position deltas immediately, if using sequential evaluation:
if (constraintParameters.evaluationOrder == Oni.ConstraintParameters.EvaluationOrder.Sequential)
{
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
BurstConstraintsBatchImpl.ApplyPositionDelta(particleIndex, constraintParameters.SORFactor, ref positions, ref deltas, ref counts);
}
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
BurstConstraintsBatchImpl.ApplyPositionDelta(particleIndex, constraintParameters.SORFactor, ref positions, ref deltas, ref counts);
}
}
contacts[i] = contact;
}
}
public void Execute(int workItemIndex)
{
int start, end;
batchData.GetConstraintRange(workItemIndex, out start, out end);
for (int i = start; i < end; ++i)
{
var contact = contacts[i];
int simplexStartA = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSizeA);
int simplexStartB = simplexCounts.GetSimplexStartAndSize(contact.bodyB, out int simplexSizeB);
// Combine collision materials:
BurstCollisionMaterial material = CombineCollisionMaterials(simplices[simplexStartA], simplices[simplexStartB]);
float4 prevPositionA = float4.zero;
float4 linearVelocityA = float4.zero;
float4 angularVelocityA = float4.zero;
float4 invInertiaTensorA = float4.zero;
quaternion orientationA = new quaternion(0, 0, 0, 0);
float simplexRadiusA = 0;
float4 prevPositionB = float4.zero;
float4 linearVelocityB = float4.zero;
float4 angularVelocityB = float4.zero;
float4 invInertiaTensorB = float4.zero;
quaternion orientationB = new quaternion(0, 0, 0, 0);
float simplexRadiusB = 0;
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
prevPositionA += prevPositions[particleIndex] * contact.pointA[j];
linearVelocityA += BurstIntegration.DifferentiateLinear(positions[particleIndex], prevPositions[particleIndex], substepTime) * contact.pointA[j];
angularVelocityA += BurstIntegration.DifferentiateAngular(orientations[particleIndex], prevOrientations[particleIndex], substepTime) * contact.pointA[j];
invInertiaTensorA += invInertiaTensors[particleIndex] * contact.pointA[j];
orientationA.value += orientations[particleIndex].value * contact.pointA[j];
simplexRadiusA += BurstMath.EllipsoidRadius(contact.normal, prevOrientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
}
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
prevPositionB += prevPositions[particleIndex] * contact.pointB[j];
linearVelocityB += BurstIntegration.DifferentiateLinear(positions[particleIndex], prevPositions[particleIndex], substepTime) * contact.pointB[j];
angularVelocityB += BurstIntegration.DifferentiateAngular(orientations[particleIndex], prevOrientations[particleIndex], substepTime) * contact.pointB[j];
invInertiaTensorB += invInertiaTensors[particleIndex] * contact.pointB[j];
orientationB.value += orientations[particleIndex].value * contact.pointB[j];
simplexRadiusB += BurstMath.EllipsoidRadius(contact.normal, prevOrientations[particleIndex], radii[particleIndex].xyz) * contact.pointB[j];
}
float4 rA = float4.zero, rB = float4.zero;
// Consider angular velocities if rolling contacts are enabled:
if (material.rollingContacts > 0)
{
rA = -contact.normal * simplexRadiusA;
rB = contact.normal * simplexRadiusB;
linearVelocityA += new float4(math.cross(angularVelocityA.xyz, rA.xyz), 0);
linearVelocityB += new float4(math.cross(angularVelocityB.xyz, rB.xyz), 0);
}
// Calculate relative velocity:
float4 relativeVelocity = linearVelocityA - linearVelocityB;
// Calculate friction impulses (in the tangent and bitangent ddirections):
float2 impulses = contact.SolveFriction(relativeVelocity, material.staticFriction, material.dynamicFriction, substepTime);
// Apply friction impulses to both particles:
if (math.abs(impulses.x) > BurstMath.epsilon || math.abs(impulses.y) > BurstMath.epsilon)
{
float4 tangentImpulse = impulses.x * contact.tangent;
float4 bitangentImpulse = impulses.y * contact.bitangent;
float4 totalImpulse = tangentImpulse + bitangentImpulse;
float baryScale = BurstMath.BaryScale(contact.pointA);
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
deltas[particleIndex] += (tangentImpulse * contact.tangentInvMassA + bitangentImpulse * contact.bitangentInvMassA) * substepTime * contact.pointA[j] * baryScale;
counts[particleIndex]++;
}
baryScale = BurstMath.BaryScale(contact.pointB);
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
deltas[particleIndex] -= (tangentImpulse * contact.tangentInvMassB + bitangentImpulse * contact.bitangentInvMassB) * substepTime * contact.pointB[j] * baryScale;
counts[particleIndex]++;
}
// Rolling contacts:
if (material.rollingContacts > 0)
{
// Calculate angular velocity deltas due to friction impulse:
float4x4 solverInertiaA = BurstMath.TransformInertiaTensor(invInertiaTensorA, orientationA);
float4x4 solverInertiaB = BurstMath.TransformInertiaTensor(invInertiaTensorB, orientationB);
float4 angVelDeltaA = math.mul(solverInertiaA, new float4(math.cross(rA.xyz, totalImpulse.xyz), 0));
float4 angVelDeltaB = -math.mul(solverInertiaB, new float4(math.cross(rB.xyz, totalImpulse.xyz), 0));
// Final angular velocities, after adding the deltas:
angularVelocityA += angVelDeltaA;
angularVelocityB += angVelDeltaB;
// Calculate weights (inverse masses):
float invMassA = math.length(math.mul(solverInertiaA, math.normalizesafe(angularVelocityA)));
float invMassB = math.length(math.mul(solverInertiaB, math.normalizesafe(angularVelocityB)));
// Calculate rolling axis and angular velocity deltas:
float4 rollAxis = float4.zero;
float rollingImpulse = contact.SolveRollingFriction(angularVelocityA, angularVelocityB, material.rollingFriction, invMassA, invMassB, ref rollAxis);
angVelDeltaA += rollAxis * rollingImpulse * invMassA;
angVelDeltaB -= rollAxis * rollingImpulse * invMassB;
// Apply orientation deltas to particles:
quaternion orientationDeltaA = BurstIntegration.AngularVelocityToSpinQuaternion(orientationA, angVelDeltaA, substepTime);
quaternion orientationDeltaB = BurstIntegration.AngularVelocityToSpinQuaternion(orientationB, angVelDeltaB, substepTime);
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
quaternion qA = orientationDeltas[particleIndex];
qA.value += orientationDeltaA.value;
orientationDeltas[particleIndex] = qA;
orientationCounts[particleIndex]++;
}
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
quaternion qB = orientationDeltas[particleIndex];
qB.value += orientationDeltaB.value;
orientationDeltas[particleIndex] = qB;
orientationCounts[particleIndex]++;
}
}
}
contacts[i] = contact;
}
}
private static void BIHTraverse(int particleIndex,
int colliderIndex,
float4 particlePosition,
quaternion particleOrientation,
float4 particleVelocity,
float4 particleRadii,
ref BurstAabb particleBounds,
int nodeIndex,
ref NativeArray <BIHNode> bihNodes,
ref NativeArray <Edge> edges,
ref NativeArray <float2> vertices,
ref EdgeMeshHeader header,
ref BurstAffineTransform colliderToSolver,
ref BurstColliderShape shape,
NativeQueue <BurstContact> .ParallelWriter contacts)
{
var node = bihNodes[header.firstNode + nodeIndex];
// amount by which we should inflate aabbs:
float offset = shape.contactOffset + particleRadii.x;
if (node.firstChild >= 0)
{
// visit min node:
if (particleBounds.min[node.axis] - offset <= node.min)
{
BIHTraverse(particleIndex, colliderIndex,
particlePosition, particleOrientation, particleVelocity, particleRadii, ref particleBounds,
node.firstChild, ref bihNodes, ref edges, ref vertices, ref header,
ref colliderToSolver, ref shape, contacts);
}
// visit max node:
if (particleBounds.max[node.axis] + offset >= node.max)
{
BIHTraverse(particleIndex, colliderIndex,
particlePosition, particleOrientation, particleVelocity, particleRadii, ref particleBounds,
node.firstChild + 1, ref bihNodes, ref edges, ref vertices, ref header,
ref colliderToSolver, ref shape, contacts);
}
}
else
{
// precalculate inverse of velocity vector for ray/aabb intersections:
float4 invDir = math.rcp(particleVelocity);
// contacts against all triangles:
for (int i = node.start; i < node.start + node.count; ++i)
{
Edge t = edges[header.firstEdge + i];
float4 v1 = new float4(vertices[header.firstVertex + t.i1], 0, 0) * colliderToSolver.scale;
float4 v2 = new float4(vertices[header.firstVertex + t.i2], 0, 0) * colliderToSolver.scale;
BurstAabb aabb = new BurstAabb(v1, v2, 0.01f);
aabb.Expand(new float4(offset));
// only generate a contact if the particle trajectory intersects its inflated aabb:
if (aabb.IntersectsRay(particlePosition, invDir, true))
{
float4 point = BurstMath.NearestPointOnEdge(v1, v2, particlePosition);
float4 pointToTri = particlePosition - point;
float distance = math.length(pointToTri);
if (distance > BurstMath.epsilon)
{
BurstContact c = new BurstContact()
{
entityA = particleIndex,
entityB = colliderIndex,
point = colliderToSolver.TransformPointUnscaled(point),
normal = colliderToSolver.TransformDirection(pointToTri / distance),
};
c.distance = distance - (shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, particleOrientation, particleRadii.xyz));
contacts.Enqueue(c);
}
}
}
}
}
public void Execute()
{
for (int i = 0; i < contacts.Length; ++i)
{
var contact = contacts[i];
// Get the indices of the particle and collider involved in this contact:
int indexA = contact.entityA;
int indexB = contact.entityB;
// Skip contacts involving triggers:
if (shapes[indexB].flags > 0)
{
continue;
}
// Get the rigidbody index (might be < 0, in that case there's no rigidbody present)
int rigidbodyIndex = shapes[indexB].rigidbodyIndex;
// Combine collision materials:
BurstCollisionMaterial material = CombineCollisionMaterials(indexA, indexB);
// Calculate relative velocity:
float4 angularVelocityA = float4.zero, rA = float4.zero, rB = float4.zero;
float4 relativeVelocity = GetRelativeVelocity(indexA, rigidbodyIndex, ref contact, ref angularVelocityA, ref rA, ref rB, material.rollingContacts > 0);
// Determine impulse magnitude:
float2 impulses = contact.SolveFriction(relativeVelocity, material.staticFriction, material.dynamicFriction, dt);
if (math.abs(impulses.x) > BurstMath.epsilon || math.abs(impulses.y) > BurstMath.epsilon)
{
float4 tangentImpulse = impulses.x * contact.tangent;
float4 bitangentImpulse = impulses.y * contact.bitangent;
float4 totalImpulse = tangentImpulse + bitangentImpulse;
deltas[indexA] += (tangentImpulse * contact.tangentInvMassA + bitangentImpulse * contact.bitangentInvMassA) * dt;
counts[indexA]++;
if (rigidbodyIndex >= 0)
{
var rb = rigidbodies[rigidbodyIndex];
float4 worldImpulse = -inertialFrame.frame.TransformVector(totalImpulse);
float4 worldPoint = inertialFrame.frame.TransformPoint(contact.point);
rigidbodyLinearDeltas[rigidbodyIndex] += rb.inverseMass * worldImpulse;
rigidbodyAngularDeltas[rigidbodyIndex] += math.mul(rb.inverseInertiaTensor, new float4(math.cross((worldPoint - rb.com).xyz, worldImpulse.xyz), 0));
}
// Rolling contacts:
if (material.rollingContacts > 0)
{
// Calculate angular velocity deltas due to friction impulse:
float4x4 solverInertiaA = BurstMath.TransformInertiaTensor(invInertiaTensors[indexA], orientations[indexA]);
float4 angVelDeltaA = math.mul(solverInertiaA, new float4(math.cross(rA.xyz, totalImpulse.xyz), 0));
float4 angVelDeltaB = float4.zero;
// Final angular velocities, after adding the deltas:
angularVelocityA += angVelDeltaA;
float4 angularVelocityB = float4.zero;
// Calculate weights (inverse masses):
float invMassA = math.length(math.mul(solverInertiaA, math.normalizesafe(angularVelocityA)));
float invMassB = 0;
if (rigidbodyIndex >= 0)
{
angVelDeltaB = math.mul(-rigidbodies[rigidbodyIndex].inverseInertiaTensor, new float4(math.cross(rB.xyz, totalImpulse.xyz), 0));
angularVelocityB = rigidbodies[rigidbodyIndex].angularVelocity + angVelDeltaB;
invMassB = math.length(math.mul(rigidbodies[rigidbodyIndex].inverseInertiaTensor, math.normalizesafe(angularVelocityB)));
}
// Calculate rolling axis and angular velocity deltas:
float4 rollAxis = float4.zero;
float rollingImpulse = contact.SolveRollingFriction(angularVelocityA, angularVelocityB, material.rollingFriction, invMassA, invMassB, ref rollAxis);
angVelDeltaA += rollAxis * rollingImpulse * invMassA;
angVelDeltaB -= rollAxis * rollingImpulse * invMassB;
// Apply orientation delta to particle:
quaternion orientationDelta = BurstIntegration.AngularVelocityToSpinQuaternion(orientations[indexA], angVelDeltaA);
quaternion qA = orientationDeltas[indexA];
qA.value += orientationDelta.value * dt;
orientationDeltas[indexA] = qA;
orientationCounts[indexA]++;
// Apply angular velocity delta to rigidbody:
if (rigidbodyIndex >= 0)
{
float4 angularDelta = rigidbodyAngularDeltas[rigidbodyIndex];
angularDelta += angVelDeltaB;
rigidbodyAngularDeltas[rigidbodyIndex] = angularDelta;
}
}
}
contacts[i] = contact;
}
}
