public void Execute(ref ModifiableContactHeader manifold, ref ModifiableContactPoint contact)
{
Entity entityA = manifold.EntityA;
Entity entityB = manifold.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;
}
if (typeA == ModifyContactJacobians.ModificationType.SurfaceVelocity || typeB == ModifyContactJacobians.ModificationType.SurfaceVelocity)
{
manifold.JacobianFlags |= JacobianFlags.EnableSurfaceVelocity;
}
if (typeA == ModifyContactJacobians.ModificationType.InfiniteInertia || typeB == ModifyContactJacobians.ModificationType.InfiniteInertia)
{
manifold.JacobianFlags |= JacobianFlags.EnableMassFactors;
}
}
public void Execute(ref ModifiableContactHeader manifold, ref ModifiableContactPoint contact)
{
if (!m_Initialized)
{
m_Initialized = true;
CurrentManifold = manifold;
NumContacts = 0;
}
// Header verification
Assert.AreEqual(manifold.CustomTagsA, (byte)0);
Assert.AreEqual(manifold.CustomTagsB, (byte)0);
Assert.AreNotEqual(manifold.BodyIndexA, manifold.BodyIndexB);
Assert.AreApproximatelyEqual(manifold.CoefficientOfFriction, 0.5f, 0.01f);
Assert.AreApproximatelyEqual(manifold.CoefficientOfRestitution, 0.0f, 0.01f);
Assert.AreEqual(manifold.ColliderKeyA.Value, ColliderKey.Empty.Value);
Assert.AreEqual(manifold.ColliderKeyB.Value, ColliderKey.Empty.Value);
Assert.AreEqual(manifold.EntityA, Bodies[manifold.BodyIndexA].Entity);
Assert.AreEqual(manifold.EntityB, Bodies[manifold.BodyIndexB].Entity);
Assert.AreEqual(manifold.JacobianFlags, (JacobianFlags)0);
Assert.IsFalse(manifold.Modified);
Assert.AreEqual(manifold.NumContacts, 4);
NumContacts++;
// Contact point verification
Assert.IsTrue(contact.Index == NumContacts - 1);
Assert.IsFalse(contact.Modified);
// Save for later verification
CurrentManifoldNumContacts[0] = CurrentManifold.NumContacts;
CurrentManifoldNumContacts[1] = NumContacts;
}
public void Execute(ref ModifiableContactHeader manifold, ref ModifiableContactPoint contact)
{
if (ConveyorBelts.HasComponent(manifold.EntityA) || ConveyorBelts.HasComponent(manifold.EntityB))
{
manifold.JacobianFlags |= JacobianFlags.EnableSurfaceVelocity;
}
}
public void Execute(ref ModifiableContactHeader manifold, ref ModifiableContactPoint contact)
{
Entity entityA = manifold.Entities.EntityA;
Entity entityB = manifold.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;
}
if (typeA == ModifyContactJacobians.ModificationType.SurfaceVelocity || typeB == ModifyContactJacobians.ModificationType.SurfaceVelocity)
{
manifold.JacobianFlags |= JacobianFlags.EnableSurfaceVelocity;
}
if (typeA == ModifyContactJacobians.ModificationType.InfiniteInertia || typeB == ModifyContactJacobians.ModificationType.InfiniteInertia)
{
manifold.JacobianFlags |= JacobianFlags.EnableMassFactors;
}
/*
* if (typeA == ModifyContactJacobians.ModificationType.ClippedImpulse || typeB == ModifyContactJacobians.ModificationType.ClippedImpulse)
* {
* //manifold.JacobianFlags |= JacobianFlags.EnableMaxImpulse;
* manifold.JacobianFlags |= JacobianFlags.
* }
*/
}
public void Execute(ref ModifiableContactHeader contactHeader, ref ModifiableContactPoint contactPoint)
{
bool isBodyA = (contactHeader.EntityA == SurfaceEntity);
bool isBodyB = (contactHeader.EntityB == SurfaceEntity);
if (isBodyA || isBodyB)
{
if (contactPoint.Index == 0)
{
// if we have a mesh surface we can get the surface normal from the plane of the polygon
var rbIdx = CollisionWorld.GetRigidBodyIndex(SurfaceEntity);
var body = CollisionWorld.Bodies[rbIdx];
if (body.Collider.Value.CollisionType == CollisionType.Composite)
{
unsafe
{
body.Collider.Value.GetLeaf(isBodyA ? contactHeader.ColliderKeyA : contactHeader.ColliderKeyB, out ChildCollider leafCollider);
if (leafCollider.Collider->Type == ColliderType.Triangle || leafCollider.Collider->Type == ColliderType.Quad)
{
PolygonCollider *polygonCollider = (PolygonCollider *)leafCollider.Collider;
// Potential optimization: If TransformFromChild has no rotation just use body.WorldFromBody.rot
// This is likely if you only have a MeshCollider with no hierarchy.
quaternion rotation = math.mul(body.WorldFromBody.rot, leafCollider.TransformFromChild.rot);
float3 surfaceNormal = math.rotate(rotation, polygonCollider->Planes[0].Normal);
distanceScale = math.dot(surfaceNormal, contactHeader.Normal);
contactHeader.Normal = surfaceNormal;
}
}
}
}
contactPoint.Distance *= distanceScale;
}
}
public void Execute(ref ModifiableContactHeader contactHeader, ref ModifiableContactPoint contactPoint)
{
bool isBodyA = (contactHeader.EntityA == SurfaceEntity);
bool isBodyB = (contactHeader.EntityB == SurfaceEntity);
if (isBodyA || isBodyB)
{
if (contactPoint.Index == 0)
{
var surfaceNormal = SurfaceNormal;
// if we have a mesh surface we can get the surface normal from the plane of the polygon
var rbIdx = CollisionWorld.GetRigidBodyIndex(SurfaceEntity);
var body = CollisionWorld.Bodies[rbIdx];
if (body.Collider.Value.Type == ColliderType.Mesh)
{
unsafe
{
var meshColliderPtr = (MeshCollider *)body.Collider.GetUnsafePtr();
meshColliderPtr->GetLeaf(isBodyA ? contactHeader.ColliderKeyA : contactHeader.ColliderKeyB, out var leafCollider);
var polygonCollider = (PolygonCollider *)leafCollider.Collider;
surfaceNormal = math.rotate(body.WorldFromBody.rot, polygonCollider->Planes[0].Normal);
}
}
var newNormal = surfaceNormal;
distanceScale = math.dot(newNormal, contactHeader.Normal);
contactHeader.Normal = newNormal;
}
contactPoint.Distance *= distanceScale;
}
}
public void Execute(ref ModifiableContactHeader header, ref ModifiableContactPoint point)
{
float3 x0 = point.Position;
float3 x1 = header.Normal * point.Distance;
OutputStreamContext->Arrow(x0, x1, UnityEngine.Color.green);
if (DisplayContactIndices)
{
OutputStreamContext->Text(point.Index.ToString().ToCharArray(), x0, UnityEngine.Color.red);
}
}
public void Execute(ref ModifiableContactHeader header, ref ModifiableContactPoint point)
{
float3 x0 = point.Position;
float3 x1 = header.Normal * point.Distance;
OutputStreamContext->Arrow(x0, x1, Unity.DebugDisplay.ColorIndex.Green);
if (DisplayContactIndices)
{
// The following line is not Burst-compatible
OutputStreamContext->Text(point.Index.ToString().ToCharArray(), x0, UnityEngine.Color.red);
}
}
public void Execute(ref ModifiableContactHeader contactHeader, ref ModifiableContactPoint contactPoint)
{
bool bUpdateNormal = (contactHeader.BodyIndexPair.BodyAIndex == m_SurfaceRBIdx) || (contactHeader.BodyIndexPair.BodyBIndex == m_SurfaceRBIdx);
if (bUpdateNormal && contactPoint.Index == 0)
{
var newNormal = m_SurfaceNormal;
distanceScale = math.dot(newNormal, contactHeader.Normal);
contactHeader.Normal = newNormal;
}
if (bUpdateNormal)
{
contactPoint.Distance *= distanceScale;
}
}
public unsafe void Execute(ref ModifiableContactHeader contactHeader,
ref ModifiableContactPoint contactPoint)
{
// only store the first contact point of any collision
// don't need any others since we only need simple collisions
if (contactPoint.Index == 0)
{
int idxA = contactHeader.BodyIndexPair.BodyAIndex;
int idxB = contactHeader.BodyIndexPair.BodyBIndex;
RigidBody rbA = world.Bodies[idxA];
RigidBody rbB = world.Bodies[idxB];
int targetIdx = -1, bulletIdx = -1;
if ((rbA.Collider->Filter.BelongsTo & targetMask) != 0 &&
(rbB.Collider->Filter.BelongsTo & bulletMask) != 0)
{
targetIdx = idxA;
bulletIdx = idxB;
}
else if ((rbB.Collider->Filter.BelongsTo & targetMask) != 0 &&
(rbA.Collider->Filter.BelongsTo & bulletMask) != 0)
{
targetIdx = idxB;
bulletIdx = idxA;
}
if (targetIdx >= 0 && bulletIdx >= 0)
{
collisions.Add(world.Bodies[targetIdx].Entity,
new CollisionInfo {
otherEnt = world.Bodies[bulletIdx].Entity,
contactPos = contactPoint.Position
});
}
}
}
public unsafe static void Execute(ref ContactsJobData <T> jobData, IntPtr additionalData,
IntPtr bufferRangePatchData, ref JobRanges jobRanges, int jobIndex)
{
var reader = new Havok.Physics.HpBlockStreamReader(jobData.ManifoldStream);
int *pluginIndexToLocal = jobData.PluginIndexToLocal->Data;
while (reader.HasItems)
{
var header = (Havok.Physics.HpManifoldStreamHeader *)reader.ReadPtr <Havok.Physics.HpManifoldStreamHeader>();
int numManifolds = header->NumManifolds;
int bodyIndexA = pluginIndexToLocal[header->BodyIds.BodyIndexA & 0x00ffffff];
int bodyIndexB = pluginIndexToLocal[header->BodyIds.BodyIndexB & 0x00ffffff];
var userHeader = new ModifiableContactHeader();
userHeader.ContactHeader.BodyPair = new BodyIndexPair
{
BodyIndexA = bodyIndexA,
BodyIndexB = bodyIndexB
};
userHeader.EntityPair = new EntityPair
{
EntityA = jobData.Bodies[bodyIndexA].Entity,
EntityB = jobData.Bodies[bodyIndexB].Entity
};
while (numManifolds-- > 0)
{
var manifold = (Havok.Physics.HpManifold *)reader.ReadPtr <Havok.Physics.HpManifold>();
userHeader.ContactHeader.NumContacts = manifold->NumPoints;
userHeader.ContactHeader.Normal = manifold->Normal.xyz;
var manifoldCache = manifold->m_CollisionCache;
userHeader.ContactHeader.CoefficientOfFriction = manifoldCache->m_friction.Value;
userHeader.ContactHeader.CoefficientOfRestitution = manifoldCache->m_restitution.Value;
userHeader.ContactHeader.ColliderKeys.ColliderKeyA.Value = manifold->m_ShapeKeyA;
userHeader.ContactHeader.ColliderKeys.ColliderKeyB.Value = manifold->m_ShapeKeyB;
Havok.Physics.HpPerManifoldProperty *cdp = manifoldCache->GetCustomPropertyStorage();
userHeader.ContactHeader.BodyCustomTags = cdp->m_bodyTagsPair;
userHeader.ContactHeader.JacobianFlags = (JacobianFlags)cdp->m_jacobianFlags;
for (int p = 0; p < manifold->NumPoints; p++)
{
var userContact = new ModifiableContactPoint
{
Index = p,
ContactPoint = new ContactPoint
{
Position = new float3(manifold->Positions[p * 4 + 0], manifold->Positions[p * 4 + 1], manifold->Positions[p * 4 + 2]),
Distance = manifold->Distances[p],
}
};
jobData.UserJobData.Execute(ref userHeader, ref userContact);
if (userContact.Modified)
{
manifold->Positions[p * 4 + 0] = userContact.ContactPoint.Position.x;
manifold->Positions[p * 4 + 1] = userContact.ContactPoint.Position.y;
manifold->Positions[p * 4 + 2] = userContact.ContactPoint.Position.z;
manifold->Distances[p] = userContact.ContactPoint.Distance;
}
if (userHeader.Modified)
{
manifold->Normal.xyz = userHeader.ContactHeader.Normal;
manifoldCache->m_friction.Value = userHeader.ContactHeader.CoefficientOfFriction;
manifoldCache->m_restitution.Value = userHeader.ContactHeader.CoefficientOfRestitution;
cdp->m_jacobianFlags = (byte)userHeader.ContactHeader.JacobianFlags;
if ((cdp->m_jacobianFlags & (byte)JacobianFlags.Disabled) != 0)
{
manifold->m_ManifoldType = 3; // hknpManifoldType::DISABLED
manifoldCache->m_collisionFlags = 1 << 9; // hknpCollisionFlags::DONT_BUILD_CONTACT_JACOBIANS
}
else
{
// Not disabled, so check for other modifications.
if ((cdp->m_jacobianFlags & (byte)JacobianFlags.EnableMassFactors) != 0)
{
manifold->m_DataFields |= 1 << 1; // hknpManifold::INERTIA_MODIFIED
manifold->m_DataFields &= 0xfb; // ~CONTAINS_TRIANGLE
var mp = (MassFactors *)UnsafeUtility.AddressOf(ref manifold->m_Scratch[0]);
mp->InverseInertiaFactorA = new float3(1);
mp->InverseMassFactorA = 1.0f;
mp->InverseInertiaFactorB = new float3(1);
mp->InverseMassFactorB = 1.0f;
}
if ((cdp->m_jacobianFlags & (byte)JacobianFlags.IsTrigger) != 0)
{
manifold->m_ManifoldType = 1; // hknpManifoldType::TRIGGER
}
if ((cdp->m_jacobianFlags & (byte)JacobianFlags.EnableSurfaceVelocity) != 0)
{
manifoldCache->m_collisionFlags |= 1 << 25; // hknpCollisionFlags::ENABLE_SURFACE_VELOCITY
}
if (userHeader.ContactHeader.CoefficientOfRestitution != 0)
{
manifoldCache->m_collisionFlags |= 1 << 20; // hknpCollisionFlags::ENABLE_RESTITUTION
}
}
}
}
}
}
}
public void Execute(ref ModifiableContactHeader header, ref ModifiableContactPoint point)
{
}
