static unsafe void InitializePlugin()
{
// Check if the shared types are compatible with the plugin
{
TypeCheckInformation typeHashes;
typeHashes.m_numTypes = 6;
typeHashes.m_typeCheckValues[0] = TypeHasher.CalculateTypeCheckValue(typeof(HavokConfiguration));
typeHashes.m_typeCheckValues[1] = TypeHasher.CalculateTypeCheckValue(typeof(HavokSimulation.StepInput));
typeHashes.m_typeCheckValues[2] = TypeHasher.CalculateTypeCheckValue(typeof(HavokSimulation.StepContext));
typeHashes.m_typeCheckValues[3] = TypeHasher.CalculateTypeCheckValue(typeof(HavokSimulation.Task));
typeHashes.m_typeCheckValues[4] = TypeHasher.CalculateTypeCheckValue(typeof(AuthInfo));
typeHashes.m_typeCheckValues[5] = TypeHasher.CalculateTypeCheckValue(typeof(Material));
// Populate struct sizes and field offsets
int fieldOffsetIndex = 0;
int structSizeIndex = 0;
typeHashes.m_structSizes[structSizeIndex++] = sizeof(HpIntArray);
fieldOffsetIndex = HpIntArray.FillFieldOffsetsArray(fieldOffsetIndex, typeHashes.m_fieldOffsets, TypeCheckInformation.fieldOffsetsLength);
// We don't care about sizes of HpBlockStream and HpBlockStream.Block
fieldOffsetIndex = HpBlockStream.FillFieldOffsetsArray(fieldOffsetIndex, typeHashes.m_fieldOffsets, TypeCheckInformation.fieldOffsetsLength);
fieldOffsetIndex = HpBlockStream.Block.FillFieldOffsetsArray(fieldOffsetIndex, typeHashes.m_fieldOffsets, TypeCheckInformation.fieldOffsetsLength);
typeHashes.m_structSizes[structSizeIndex++] = sizeof(HpManifoldStreamHeader);
fieldOffsetIndex = HpManifoldStreamHeader.FillFieldOffsetsArray(fieldOffsetIndex, typeHashes.m_fieldOffsets, TypeCheckInformation.fieldOffsetsLength);
typeHashes.m_structSizes[structSizeIndex++] = sizeof(HpManifold);
fieldOffsetIndex = HpManifold.FillFieldOffsetsArray(fieldOffsetIndex, typeHashes.m_fieldOffsets, TypeCheckInformation.fieldOffsetsLength);
typeHashes.m_structSizes[structSizeIndex++] = sizeof(HPManifoldCollisionCache);
fieldOffsetIndex = HPManifoldCollisionCache.FillFieldOffsetsArray(fieldOffsetIndex, typeHashes.m_fieldOffsets, TypeCheckInformation.fieldOffsetsLength);
typeHashes.m_structSizes[structSizeIndex++] = sizeof(HpJacAngular);
fieldOffsetIndex = HpJacAngular.FillFieldOffsetsArray(fieldOffsetIndex, typeHashes.m_fieldOffsets, TypeCheckInformation.fieldOffsetsLength);
typeHashes.m_structSizes[structSizeIndex++] = sizeof(HpJac3dFriction);
fieldOffsetIndex = HpJac3dFriction.FillFieldOffsetsArray(fieldOffsetIndex, typeHashes.m_fieldOffsets, TypeCheckInformation.fieldOffsetsLength);
// Only size is important for HpJacModHeader (fields are not used)
typeHashes.m_structSizes[structSizeIndex++] = sizeof(HpJacModHeader);
typeHashes.m_structSizes[structSizeIndex++] = sizeof(HpJacHeader);
fieldOffsetIndex = HpJacHeader.FillFieldOffsetsArray(fieldOffsetIndex, typeHashes.m_fieldOffsets, TypeCheckInformation.fieldOffsetsLength);
// HpCsContactJacRange also covers HpLinkedRange and HpBlockStreamRange
typeHashes.m_structSizes[structSizeIndex++] = sizeof(HpCsContactJacRange);
fieldOffsetIndex = HpCsContactJacRange.FillFieldOffsetsArray(fieldOffsetIndex, typeHashes.m_fieldOffsets, TypeCheckInformation.fieldOffsetsLength);
// We don't care about the size of HpGrid
fieldOffsetIndex = HpGrid.FillFieldOffsetsArray(fieldOffsetIndex, typeHashes.m_fieldOffsets, TypeCheckInformation.fieldOffsetsLength);
bool isCompatible = false;
try
{
isCompatible = HP_CheckCompatibility(ref typeHashes);
UnityEditor.EditorApplication.update -= InitializePlugin;
}
catch (DllNotFoundException)
{
// Try again during next editor update; DLL copy might be in progress
return;
}
if (!isCompatible)
{
throw new InvalidOperationException("Your Havok.Physics plugin is incompatible with this version");
}
}
// Try to unlock the plugin in the background whenever a user has logged in (including during startup)
//s_userLoginMonitor = new UserLoginMonitor();
//s_userLoginMonitor.UserLoginStateChanged += (object o, bool newLoginState) =>
//{
// if (newLoginState)
// {
// TryUnlockAsync();
// }
//};
}
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;
}
}
