internal static unsafe void ExecuteImpl(ref PhysicsWorld world, float timeStep,
NativeArray <DispatchPairSequencer.DispatchPair> dispatchPairs,
int dispatchPairReadOffset, int numPairsToRead, ref NativeStream.Writer contactWriter)
{
for (int i = 0; i < numPairsToRead; i++)
{
DispatchPairSequencer.DispatchPair dispatchPair = dispatchPairs[dispatchPairReadOffset + i];
// Invalid pairs can exist by being disabled by users
if (dispatchPair.IsValid)
{
if (dispatchPair.IsContact)
{
// Create contact manifolds for this pair of bodies
var pair = new BodyIndexPair
{
BodyIndexA = dispatchPair.BodyIndexA,
BodyIndexB = dispatchPair.BodyIndexB
};
RigidBody rigidBodyA = world.Bodies[pair.BodyIndexA];
RigidBody rigidBodyB = world.Bodies[pair.BodyIndexB];
MotionVelocity motionVelocityA = pair.BodyIndexA < world.MotionVelocities.Length ?
world.MotionVelocities[pair.BodyIndexA] : MotionVelocity.Zero;
MotionVelocity motionVelocityB = pair.BodyIndexB < world.MotionVelocities.Length ?
world.MotionVelocities[pair.BodyIndexB] : MotionVelocity.Zero;
ManifoldQueries.BodyBody(rigidBodyA, rigidBodyB, motionVelocityA, motionVelocityB,
world.CollisionWorld.CollisionTolerance, timeStep, pair, ref contactWriter);
}
}
}
}
public unsafe void Execute(int workItemIndex)
{
int dispatchPairReadOffset = SolverSchedulerInfo.GetWorkItemReadOffset(workItemIndex, out int numPairsToRead);
ContactWriter.BeginForEachIndex(workItemIndex);
JointJacobianWriter.BeginForEachIndex(workItemIndex);
for (int i = 0; i < numPairsToRead; i++)
{
Scheduler.DispatchPair dispatchPair = PhasedDispatchPairs[dispatchPairReadOffset + i];
// Invalid pairs can exist by being disabled by users
if (dispatchPair.IsValid)
{
if (dispatchPair.IsContact)
{
// Create contact manifolds for this pair of bodies
var pair = new BodyIndexPair
{
BodyAIndex = dispatchPair.BodyAIndex,
BodyBIndex = dispatchPair.BodyBIndex
};
RigidBody rigidBodyA = World.Bodies[pair.BodyAIndex];
RigidBody rigidBodyB = World.Bodies[pair.BodyBIndex];
MotionVelocity motionVelocityA = pair.BodyAIndex < World.MotionVelocities.Length ?
World.MotionVelocities[pair.BodyAIndex] : MotionVelocity.Zero;
MotionVelocity motionVelocityB = pair.BodyBIndex < World.MotionVelocities.Length ?
World.MotionVelocities[pair.BodyBIndex] : MotionVelocity.Zero;
ManifoldQueries.BodyBody(rigidBodyA, rigidBodyB, motionVelocityA, motionVelocityB,
World.CollisionWorld.CollisionTolerance, TimeStep, pair, ref ContactWriter);
}
else
{
Joint joint = World.Joints[dispatchPair.JointIndex];
// Need to fetch the real body indices from the joint, as the scheduler may have reordered them
int bodyAIndex = joint.BodyPair.BodyAIndex;
int bodyBIndex = joint.BodyPair.BodyBIndex;
GetMotion(ref World, bodyAIndex, out MotionVelocity velocityA, out MotionData motionA);
GetMotion(ref World, bodyBIndex, out MotionVelocity velocityB, out MotionData motionB);
Solver.BuildJointJacobian(joint.JointData, joint.BodyPair, velocityA, velocityB, motionA, motionB, TimeStep, NumIterations, ref JointJacobianWriter);
}
}
}
JointJacobianWriter.EndForEachIndex();
ContactWriter.EndForEachIndex();
}
public unsafe static void Execute(ref BodyPairsJobData <T> jobData, IntPtr additionalData,
IntPtr bufferRangePatchData, ref JobRanges ranges, int jobIndex)
{
if (jobData.BlockStreamStart == null || !jobData.BlockStreamStart->HasElements)
{
return;
}
var blockStreamReader = new Havok.Physics.HpBlockStreamReader(jobData.BlockStreamStart);
int *pluginIndexToLocal = jobData.PluginIndexToLocal->Data;
while (blockStreamReader.HasItems)
{
BodyIndexPair indices = blockStreamReader.Read <BodyIndexPair>(); // Really an hknpBodyIdPair
int bodyIndexA = pluginIndexToLocal[indices.BodyIndexA & 0x00ffffff];
int bodyIndexB = pluginIndexToLocal[indices.BodyIndexB & 0x00ffffff];
var pair = new ModifiableBodyPair
{
BodyIndexPair = new BodyIndexPair {
BodyIndexA = bodyIndexA, BodyIndexB = bodyIndexB
},
EntityPair = new EntityPair
{
EntityA = jobData.Bodies[bodyIndexA].Entity,
EntityB = jobData.Bodies[bodyIndexB].Entity
}
};
jobData.UserJobData.Execute(ref pair);
if (pair.BodyIndexA == -1 || pair.BodyIndexB == -1)
{
blockStreamReader.Write(BodyIndexPair.Invalid);
}
}
}
public void Disable()
{
BodyIndexPair = BodyIndexPair.Invalid;
}
// Write a set of contact manifolds for a pair of bodies to the given stream.
public static unsafe void BodyBody(RigidBody rigidBodyA, RigidBody rigidBodyB, MotionVelocity motionVelocityA, MotionVelocity motionVelocityB,
float collisionTolerance, float timeStep, BodyIndexPair pair, ref NativeStream.Writer contactWriter)
{
Collider *colliderA = rigidBodyA.Collider;
Collider *colliderB = rigidBodyB.Collider;
if (colliderA == null || colliderB == null || !CollisionFilter.IsCollisionEnabled(colliderA->Filter, colliderB->Filter))
{
return;
}
// Build combined motion expansion
MotionExpansion expansion;
{
MotionExpansion expansionA = motionVelocityA.CalculateExpansion(timeStep);
MotionExpansion expansionB = motionVelocityB.CalculateExpansion(timeStep);
expansion = new MotionExpansion
{
Linear = expansionA.Linear - expansionB.Linear,
Uniform = expansionA.Uniform + expansionB.Uniform + collisionTolerance
};
}
var context = new Context
{
BodyIndices = pair,
BodyCustomTags = new CustomTagsPair {
CustomTagsA = rigidBodyA.CustomTags, CustomTagsB = rigidBodyB.CustomTags
},
BodiesHaveInfiniteMass =
!math.any(motionVelocityA.InverseInertiaAndMass) &&
!math.any(motionVelocityB.InverseInertiaAndMass),
ContactWriter = (NativeStream.Writer *)UnsafeUtility.AddressOf(ref contactWriter)
};
var worldFromA = new MTransform(rigidBodyA.WorldFromBody);
var worldFromB = new MTransform(rigidBodyB.WorldFromBody);
// Dispatch to appropriate manifold generator
switch (colliderA->CollisionType)
{
case CollisionType.Convex:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
ConvexConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
case CollisionType.Composite:
ConvexComposite(context, ColliderKey.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion, false);
break;
case CollisionType.Terrain:
ConvexTerrain(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
}
break;
case CollisionType.Composite:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
CompositeConvex(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false);
break;
case CollisionType.Composite:
CompositeComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false);
break;
case CollisionType.Terrain:
CompositeTerrain(context, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
}
break;
case CollisionType.Terrain:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
TerrainConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
case CollisionType.Composite:
TerrainComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
case CollisionType.Terrain:
UnityEngine.Assertions.Assert.IsTrue(false);
break;
}
break;
}
}
// Write a set of contact manifolds for a pair of bodies to the given stream.
public static unsafe void BodyBody(ref PhysicsWorld world, BodyIndexPair pair, float timeStep, ref BlockStream.Writer contactWriter)
{
RigidBody rigidBodyA = world.Bodies[pair.BodyAIndex];
RigidBody rigidBodyB = world.Bodies[pair.BodyBIndex];
Collider *colliderA = rigidBodyA.Collider;
Collider *colliderB = rigidBodyB.Collider;
if (colliderA == null || colliderB == null || !CollisionFilter.IsCollisionEnabled(colliderA->Filter, colliderB->Filter))
{
return;
}
// Build combined motion expansion
MotionExpansion expansion;
{
MotionExpansion GetBodyExpansion(int bodyIndex, NativeSlice <MotionVelocity> mvs)
{
return(bodyIndex < mvs.Length ? mvs[bodyIndex].CalculateExpansion(timeStep) : MotionExpansion.Zero);
}
MotionExpansion expansionA = GetBodyExpansion(pair.BodyAIndex, world.MotionVelocities);
MotionExpansion expansionB = GetBodyExpansion(pair.BodyBIndex, world.MotionVelocities);
expansion = new MotionExpansion
{
Linear = expansionA.Linear - expansionB.Linear,
Uniform = expansionA.Uniform + expansionB.Uniform + world.CollisionTolerance
};
}
var context = new Context
{
BodyIndices = pair,
BodyCustomDatas = new CustomDataPair {
CustomDataA = rigidBodyA.CustomData, CustomDataB = rigidBodyB.CustomData
}
};
var worldFromA = new MTransform(rigidBodyA.WorldFromBody);
var worldFromB = new MTransform(rigidBodyB.WorldFromBody);
// Dispatch to appropriate manifold generator
switch (colliderA->CollisionType)
{
case CollisionType.Convex:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
ConvexConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false, ref contactWriter);
break;
case CollisionType.Composite:
ConvexComposite(context, ColliderKey.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion, false, ref contactWriter);
break;
}
break;
case CollisionType.Composite:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
CompositeConvex(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false, ref contactWriter);
break;
case CollisionType.Composite:
CompositeComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false, ref contactWriter);
break;
}
break;
}
}
