public void Execute(ref ModifiableJacobianHeader header, ref ModifiableContactJacobian jacobian) { // Header verification Assert.IsFalse(header.AngularChanged); Assert.AreNotEqual(header.BodyPair.BodyAIndex, header.BodyPair.BodyBIndex); Assert.AreEqual(header.ColliderKeys.ColliderKeyA.Value, ColliderKey.Empty.Value); Assert.AreEqual(header.ColliderKeys.ColliderKeyB.Value, ColliderKey.Empty.Value); Assert.AreEqual(header.Entities.EntityA, Bodies[header.BodyPair.BodyAIndex].Entity); Assert.AreEqual(header.Entities.EntityB, Bodies[header.BodyPair.BodyBIndex].Entity); Assert.AreEqual(header.Flags, (JacobianFlags)0); Assert.IsFalse(header.HasColliderKeys); Assert.IsFalse(header.HasMassFactors); Assert.IsFalse(header.HasSurfaceVelocity); Assert.IsFalse(header.ModifiersChanged); Assert.AreEqual(header.Type, JacobianType.Contact); // Jacobian verification Assert.AreApproximatelyEqual(jacobian.CoefficientOfFriction, 0.5f, 0.01f); Assert.IsFalse(jacobian.Modified); Assert.AreEqual(jacobian.NumContacts, 4); for (int i = 0; i < jacobian.NumContacts; i++) { ContactJacAngAndVelToReachCp jacAng = header.GetAngularJacobian(i); Assert.AreEqual(jacAng.Jac.Impulse, 0.0f); } }
public void Execute(ref ModifiableJacobianHeader jacHeader, ref ModifiableContactJacobian contactJacobian) { if (!jacHeader.HasSurfaceVelocity) { return; } float3 linearVelocity = float3.zero; float3 angularVelocity = float3.zero; // Get the surface velocities if available for (int i = 0; i < 2; i++) { var entity = (i == 0) ? jacHeader.EntityA : jacHeader.EntityB; if (!ConveyorBelts.HasComponent(entity)) { continue; } var index = (i == 0) ? jacHeader.BodyIndexA : jacHeader.BodyIndexB; var rotation = Bodies[index].WorldFromBody.rot; var belt = ConveyorBelts[entity]; if (belt.IsAngular) { // assuming rotation is around contact normal. var av = contactJacobian.Normal * belt.Speed; // calculate linear velocity at point, assuming rotating around body pivot var otherIndex = (i == 0) ? jacHeader.BodyIndexB : jacHeader.BodyIndexA; var offset = Bodies[otherIndex].WorldFromBody.pos - Bodies[index].WorldFromBody.pos; var lv = math.cross(av, offset); angularVelocity += av; linearVelocity += lv; } else { linearVelocity += math.rotate(rotation, belt.LocalDirection) * belt.Speed; } } // Add the extra velocities jacHeader.SurfaceVelocity = new SurfaceVelocity { LinearVelocity = jacHeader.SurfaceVelocity.LinearVelocity + linearVelocity, AngularVelocity = jacHeader.SurfaceVelocity.AngularVelocity + angularVelocity, }; }
public void Execute(ref ModifiableJacobianHeader jacHeader, ref ModifiableContactJacobian contactJacobian) { Entity entityA = jacHeader.EntityA; Entity entityB = jacHeader.EntityB; ModifyContactJacobians.ModificationType typeA = ModifyContactJacobians.ModificationType.None; if (modificationData.HasComponent(entityA)) { typeA = modificationData[entityA].type; } ModifyContactJacobians.ModificationType typeB = ModifyContactJacobians.ModificationType.None; if (modificationData.HasComponent(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, or friction should be disabled through jacobian if (typeA == ModifyContactJacobians.ModificationType.NoAngularEffects || typeB == ModifyContactJacobians.ModificationType.NoAngularEffects || typeA == ModifyContactJacobians.ModificationType.DisabledAngularFriction || typeB == ModifyContactJacobians.ModificationType.DisabledAngularFriction) { // 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 jacHeader.SurfaceVelocity = new SurfaceVelocity { LinearVelocity = float3.zero, AngularVelocity = contactJacobian.Normal * (new float3(0.0f, 1.0f, 0.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 { InverseInertiaFactorA = float3.zero, InverseMassFactorA = 1.0f, InverseInertiaFactorB = float3.zero, InverseMassFactorB = 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); } }
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); } }
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; } }