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;
}
}
