internal static unsafe void ExecuteImpl(int index, sfloat aabbMargin,
NativeArray <RigidBody> rigidBodies, NativeArray <Aabb> aabbs, NativeArray <PointAndIndex> points,
NativeArray <CollisionFilter> filtersOut, NativeArray <bool> respondsToCollisionOut)
{
RigidBody body = rigidBodies[index];
Aabb aabb;
if (body.Collider.IsCreated)
{
aabb = body.Collider.Value.CalculateAabb(body.WorldFromBody);
aabb.Expand(aabbMargin);
filtersOut[index] = body.Collider.Value.Filter;
respondsToCollisionOut[index] = body.Collider.Value.RespondsToCollision;
}
else
{
aabb.Min = body.WorldFromBody.pos;
aabb.Max = body.WorldFromBody.pos;
filtersOut[index] = CollisionFilter.Zero;
respondsToCollisionOut[index] = false;
}
aabbs[index] = aabb;
points[index] = new BoundingVolumeHierarchy.PointAndIndex
{
Position = aabb.Center,
Index = index
};
}
private unsafe int BuildBoundingVolume(out NativeArray <BoundingVolumeHierarchy.Node> nodes)
{
// Create inputs
var points = new NativeArray <BoundingVolumeHierarchy.PointAndIndex>(NumChildren, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var aabbs = new NativeArray <Aabb>(NumChildren, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
for (int i = 0; i < NumChildren; ++i)
{
points[i] = new BoundingVolumeHierarchy.PointAndIndex {
Position = Children[i].CompoundFromChild.pos, Index = i
};
aabbs[i] = Children[i].Collider->CalculateAabb(Children[i].CompoundFromChild);
}
// Build BVH
// Todo: cleanup, better size of nodes array
nodes = new NativeArray <BoundingVolumeHierarchy.Node>(2 + NumChildren, Allocator.Temp, NativeArrayOptions.UninitializedMemory)
{
[0] = BoundingVolumeHierarchy.Node.Empty,
[1] = BoundingVolumeHierarchy.Node.Empty
};
var bvh = new BoundingVolumeHierarchy(nodes);
bvh.Build(points, aabbs, out int numNodes);
return(numNodes);
}
internal static unsafe void ExecuteImpl(int index, sfloat aabbMargin, float3 gravity, sfloat timeStep,
NativeArray <RigidBody> rigidBodies, NativeArray <MotionVelocity> motionVelocities,
NativeArray <Aabb> aabbs, NativeArray <PointAndIndex> points,
NativeArray <CollisionFilter> filtersOut, NativeArray <bool> respondsToCollisionOut)
{
RigidBody body = rigidBodies[index];
Aabb aabb;
if (body.Collider.IsCreated)
{
var mv = motionVelocities[index];
// Apply gravity only on a copy to get proper expansion for the AABB,
// actual applying of gravity will be done later in the physics step
mv.LinearVelocity += gravity * timeStep * mv.GravityFactor;
MotionExpansion expansion = mv.CalculateExpansion(timeStep);
aabb = expansion.ExpandAabb(body.Collider.Value.CalculateAabb(body.WorldFromBody));
aabb.Expand(aabbMargin);
filtersOut[index] = body.Collider.Value.Filter;
respondsToCollisionOut[index] = body.Collider.Value.RespondsToCollision;
}
else
{
aabb.Min = body.WorldFromBody.pos;
aabb.Max = body.WorldFromBody.pos;
filtersOut[index] = CollisionFilter.Zero;
respondsToCollisionOut[index] = false;
}
aabbs[index] = aabb;
points[index] = new BoundingVolumeHierarchy.PointAndIndex
{
Position = aabb.Center,
Index = index
};
}
