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 simplexStart = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSize); int colliderIndex = contact.bodyB; // Skip contacts involving triggers: if (shapes[colliderIndex].flags > 0) { continue; } // Get the rigidbody index (might be < 0, in that case there's no rigidbody present) int rigidbodyIndex = shapes[colliderIndex].rigidbodyIndex; // Combine collision materials (use material from first particle in simplex) BurstCollisionMaterial material = CombineCollisionMaterials(simplices[simplexStart], colliderIndex); // Calculate relative velocity: float4 rA = float4.zero, rB = float4.zero; 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; for (int j = 0; j < simplexSize; ++j) { int particleIndex = simplices[simplexStart + 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]; } float4 relativeVelocity = linearVelocityA; // Add particle angular velocity if rolling contacts are enabled: if (material.rollingContacts > 0) { rA = -contact.normal * simplexRadiusA; 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.pointB) - rigidbodies[rigidbodyIndex].com; relativeVelocity -= BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, contact.pointB, rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame); } // Determine impulse magnitude: float2 impulses = contact.SolveFriction(relativeVelocity, material.staticFriction, material.dynamicFriction, stepTime); 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 < simplexSize; ++j) { int particleIndex = simplices[simplexStart + j]; //(tangentImpulse * contact.tangentInvMassA + bitangentImpulse * contact.bitangentInvMassA) * dt; deltas[particleIndex] += (tangentImpulse * contact.tangentInvMassA + bitangentImpulse * contact.bitangentInvMassA) * substepTime * contact.pointA[j] * baryScale; counts[particleIndex]++; } if (rigidbodyIndex >= 0) { BurstMath.ApplyImpulse(rigidbodyIndex, -totalImpulse, contact.pointB, rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame); } // Rolling contacts: if (material.rollingContacts > 0) { // Calculate angular velocity deltas due to friction impulse: float4x4 solverInertiaA = BurstMath.TransformInertiaTensor(invInertiaTensorA, orientationA); 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 particles: quaternion orientationDelta = BurstIntegration.AngularVelocityToSpinQuaternion(orientationA, angVelDeltaA, substepTime); for (int j = 0; j < simplexSize; ++j) { int particleIndex = simplices[simplexStart + j]; quaternion qA = orientationDeltas[particleIndex]; qA.value += orientationDelta.value; orientationDeltas[particleIndex] = qA; orientationCounts[particleIndex]++; } // Apply angular velocity delta to rigidbody: if (rigidbodyIndex >= 0) { float4 angularDelta = rigidbodyAngularDeltas[rigidbodyIndex]; angularDelta += angVelDeltaB; rigidbodyAngularDeltas[rigidbodyIndex] = angularDelta; } } } contacts[i] = contact; } }
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 / (substepTime * substepTime); // project particle position to the end of the full step: float4 particlePosition = math.lerp(prevPositions[particleIndex], positions[particleIndex], substeps); // 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; if (rigidbodyIndex >= 0) { var rigidbody = rigidbodies[rigidbodyIndex]; // predict offset point position: float4 velocityAtPoint = BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, inertialFrame.frame.InverseTransformPoint(worldPinOffset), rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame); predictedPinOffset = BurstIntegration.IntegrateLinear(predictedPinOffset, inertialFrame.frame.TransformVector(velocityAtPoint), stepTime); // predict rotation at the end of the step: predictedRotation = BurstIntegration.IntegrateAngular(predictedRotation, rigidbody.angularVelocity + rigidbodyAngularDeltas[rigidbodyIndex], stepTime); // 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); predictedRotation = math.mul(math.conjugate(inertialFrame.frame.rotation), predictedRotation); 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] / substeps; counts[particleIndex]++; if (rigidbodyIndex >= 0) { BurstMath.ApplyImpulse(rigidbodyIndex, -correction / stepTime * 1, inertialFrame.frame.InverseTransformPoint(worldPinOffset), rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame); } 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] / substeps; orientationDeltas[particleIndex] = orientDelta; orientationCounts[particleIndex]++; if (rigidbodyIndex >= 0) { BurstMath.ApplyDeltaQuaternion(rigidbodyIndex, predictedRotation, -math.mul(orientations[particleIndex], dlambdaQ).value *rigidbodyAngularW, rigidbodyAngularDeltas, inertialFrame.frame, stepTime); } } lambdas[i] = lambda; } }
public void Execute() { for (int i = 0; i < contacts.Length; ++i) { var contact = contacts[i]; int simplexStart = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSize); int colliderIndex = contact.bodyB; // Skip contacts involving triggers: if (shapes[colliderIndex].flags > 0) { continue; } // Get the rigidbody index (might be < 0, in that case there's no rigidbody present) int rigidbodyIndex = shapes[colliderIndex].rigidbodyIndex; // Combine collision materials (use material from first particle in simplex) BurstCollisionMaterial material = CombineCollisionMaterials(simplices[simplexStart], colliderIndex); // Get relative velocity at contact point. // 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. float4 simplexPosition = float4.zero; float4 simplexPrevPosition = float4.zero; float simplexRadius = 0; for (int j = 0; j < simplexSize; ++j) { int particleIndex = simplices[simplexStart + j]; simplexPosition += positions[particleIndex] * contact.pointA[j]; simplexPrevPosition += prevPositions[particleIndex] * contact.pointA[j]; simplexRadius += BurstMath.EllipsoidRadius(contact.normal, orientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j]; } // project position to the end of the full step: float4 posA = math.lerp(simplexPrevPosition, simplexPosition, substeps); posA += -contact.normal * simplexRadius; float4 posB = contact.pointB; if (rigidbodyIndex >= 0) { posB += BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, contact.pointB, rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame) * stepTime; } // adhesion: float lambda = contact.SolveAdhesion(posA, posB, material.stickDistance, material.stickiness, stepTime); // depenetration: lambda += contact.SolvePenetration(posA, posB, solverParameters.maxDepenetration * stepTime); // Apply normal impulse to both simplex and rigidbody: if (math.abs(lambda) > BurstMath.epsilon) { float4 delta = lambda * contact.normal * BurstMath.BaryScale(contact.pointA) / substeps; for (int j = 0; j < simplexSize; ++j) { int particleIndex = simplices[simplexStart + j]; deltas[particleIndex] += delta * invMasses[particleIndex] * contact.pointA[j]; counts[particleIndex]++; } // Apply position deltas immediately, if using sequential evaluation: if (constraintParameters.evaluationOrder == Oni.ConstraintParameters.EvaluationOrder.Sequential) { for (int j = 0; j < simplexSize; ++j) { int particleIndex = simplices[simplexStart + j]; BurstConstraintsBatchImpl.ApplyPositionDelta(particleIndex, constraintParameters.SORFactor, ref positions, ref deltas, ref counts); } } if (rigidbodyIndex >= 0) { BurstMath.ApplyImpulse(rigidbodyIndex, -lambda / stepTime * contact.normal, contact.pointB, rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame); } } contacts[i] = contact; } }