public static void Contacts(int particleIndex,
int colliderIndex,
float4 particlePosition,
quaternion particleOrientation,
float4 particleVelocity,
float4 particleRadii,
float deltaTime,
ref NativeArray <BIHNode> bihNodes,
ref NativeArray <Edge> edges,
ref NativeArray <float2> vertices,
EdgeMeshHeader header,
ref BurstAffineTransform colliderToSolver,
ref BurstColliderShape shape,
NativeQueue <BurstContact> .ParallelWriter contacts)
{
float4 colliderSpacePosition = colliderToSolver.InverseTransformPoint(particlePosition);
float4 colliderSpaceVel = colliderToSolver.InverseTransformVector(particleVelocity * deltaTime);
BurstAabb particleBounds = new BurstAabb(colliderSpacePosition,
colliderSpacePosition + colliderSpaceVel,
particleRadii.x / math.cmax(colliderToSolver.scale));
colliderSpacePosition *= colliderToSolver.scale;
BIHTraverse(particleIndex, colliderIndex,
colliderSpacePosition, particleOrientation, colliderSpaceVel, particleRadii, ref particleBounds,
0, ref bihNodes, ref edges, ref vertices, ref header, ref colliderToSolver, ref shape, contacts);
}
public static void Contacts(int particleIndex,
int colliderIndex,
float4 position,
quaternion orientation,
float4 radii,
ref NativeArray <BurstDFNode> dfNodes,
DistanceFieldHeader header,
BurstAffineTransform colliderToSolver,
BurstColliderShape shape,
NativeQueue <BurstContact> .ParallelWriter contacts)
{
float4 pos = colliderToSolver.InverseTransformPoint(position);
BurstContact c = new BurstContact
{
entityA = particleIndex,
entityB = colliderIndex,
};
float4 sample = DFTraverse(pos, 0, ref header, ref dfNodes);
c.normal = new float4(math.normalize(sample.xyz), 0);
c.point = pos - c.normal * sample[3];
c.normal = colliderToSolver.TransformDirection(c.normal);
c.point = colliderToSolver.TransformPoint(c.point);
c.distance = sample[3] * math.cmax(colliderToSolver.scale.xyz) - (shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz));
contacts.Enqueue(c);
}
public static void Contacts(int particleIndex,
float4 position,
quaternion orientation,
float4 radii,
int colliderIndex,
BurstAffineTransform transform,
BurstColliderShape shape,
NativeQueue <BurstContact> .ParallelWriter contacts)
{
float4 center = shape.center * transform.scale;
position = transform.InverseTransformPointUnscaled(position) - center;
float radius = shape.size.x * math.cmax(transform.scale.xyz);
float distanceToCenter = math.length(position);
float4 normal = position / distanceToCenter;
BurstContact c = new BurstContact
{
entityA = particleIndex,
entityB = colliderIndex,
point = center + normal * radius,
normal = normal,
};
c.point = transform.TransformPointUnscaled(c.point);
c.normal = transform.TransformDirection(c.normal);
c.distance = distanceToCenter - radius - (shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz));
contacts.Enqueue(c);
}
private void GenerateContacts(ColliderShape.ShapeType colliderType,
int particleIndex,
int colliderIndex,
float4 particlePosition,
quaternion particleOrientation,
float4 particleVelocity,
float4 particleRadii,
BurstAffineTransform colliderToSolver,
BurstColliderShape shape,
NativeQueue <BurstContact> .ParallelWriter contacts,
float dt)
{
switch (colliderType)
{
case ColliderShape.ShapeType.Sphere:
BurstSphere.Contacts(particleIndex, particlePosition, particleOrientation, particleRadii,
colliderIndex, colliderToSolver, shape,
contacts);
break;
case ColliderShape.ShapeType.Box:
BurstBox.Contacts(particleIndex, particlePosition, particleOrientation, particleRadii,
colliderIndex, colliderToSolver, shape,
contacts);
break;
case ColliderShape.ShapeType.Capsule:
BurstCapsule.Contacts(particleIndex, particlePosition, particleOrientation, particleRadii,
colliderIndex, colliderToSolver, shape,
contacts);
break;
case ColliderShape.ShapeType.SignedDistanceField:
BurstDistanceField.Contacts(particleIndex, colliderIndex,
particlePosition, particleOrientation, particleRadii,
ref dfNodes, distanceFieldHeaders[shape.dataIndex], colliderToSolver, shape, contacts);
break;
case ColliderShape.ShapeType.Heightmap:
BurstHeightField.Contacts(particleIndex, colliderIndex,
particlePosition, particleOrientation, particleRadii,
ref heightFieldSamples, heightFieldHeaders[shape.dataIndex], colliderToSolver, shape, contacts);
break;
case ColliderShape.ShapeType.TriangleMesh:
BurstTriangleMesh.Contacts(particleIndex, colliderIndex,
particlePosition, particleOrientation, particleVelocity, particleRadii, dt,
ref bihNodes, ref triangles, ref vertices, triangleMeshHeaders[shape.dataIndex],
ref colliderToSolver, ref shape, contacts);
break;
case ColliderShape.ShapeType.EdgeMesh:
BurstEdgeMesh.Contacts(particleIndex, colliderIndex,
particlePosition, particleOrientation, particleVelocity, particleRadii, dt,
ref edgeBihNodes, ref edges, ref edgeVertices, edgeMeshHeaders[shape.dataIndex],
ref colliderToSolver, ref shape, contacts);
break;
}
}
public BurstInertialFrame(float4 position, float4 scale, quaternion rotation)
{
this.frame = new BurstAffineTransform(position, rotation, scale);
this.prevFrame = frame;
velocity = float4.zero;
angularVelocity = float4.zero;
acceleration = float4.zero;
angularAcceleration = float4.zero;
}
public BurstInertialFrame(BurstAffineTransform frame)
{
this.frame = frame;
this.prevFrame = frame;
velocity = float4.zero;
angularVelocity = float4.zero;
acceleration = float4.zero;
angularAcceleration = float4.zero;
}
public static void ApplyImpulse(int rigidbodyIndex,
float4 impulse,
float4 point,
NativeArray <BurstRigidbody> rigidbodies,
NativeArray <float4> linearDeltas,
NativeArray <float4> angularDeltas,
BurstAffineTransform transform)
{
float4 impulseWS = transform.TransformVector(impulse);
float4 r = transform.TransformPoint(point) - rigidbodies[rigidbodyIndex].com;
linearDeltas[rigidbodyIndex] += rigidbodies[rigidbodyIndex].inverseMass * impulseWS;
angularDeltas[rigidbodyIndex] += math.mul(rigidbodies[rigidbodyIndex].inverseInertiaTensor, new float4(math.cross(r.xyz, impulseWS.xyz), 0));
}
public static void ApplyDeltaQuaternion(int rigidbodyIndex,
quaternion rotation,
quaternion delta,
NativeArray <float4> angularDeltas,
BurstAffineTransform transform,
float dt)
{
quaternion rotationWS = math.mul(transform.rotation, rotation);
quaternion deltaWS = math.mul(transform.rotation, delta);
// convert quaternion delta to angular acceleration:
quaternion newRotation = math.normalize(new quaternion(rotationWS.value + deltaWS.value));
angularDeltas[rigidbodyIndex] += BurstIntegration.DifferentiateAngular(newRotation, rotationWS, dt);
}
public static float4 GetRigidbodyVelocityAtPoint(int rigidbodyIndex,
float4 point,
NativeArray <BurstRigidbody> rigidbodies,
NativeArray <float4> linearDeltas,
NativeArray <float4> angularDeltas,
BurstAffineTransform transform)
{
float4 linear = rigidbodies[rigidbodyIndex].velocity + linearDeltas[rigidbodyIndex];
float4 angular = rigidbodies[rigidbodyIndex].angularVelocity + angularDeltas[rigidbodyIndex];
float4 r = transform.TransformPoint(point) - rigidbodies[rigidbodyIndex].com;
// Point is assumed to be expressed in solver space. Since rigidbodies are expressed in world space, we need to convert the
// point to world space, and convert the resulting velocity back to solver space.
return(transform.InverseTransformVector(linear + new float4(math.cross(angular.xyz, r.xyz), 0)));
}
public void Update(float4 position, float4 scale, quaternion rotation, float dt)
{
prevFrame = frame;
float4 prevVelocity = velocity;
float4 prevAngularVelocity = angularVelocity;
frame.translation = position;
frame.rotation = rotation;
frame.scale = scale;
velocity = BurstIntegration.DifferentiateLinear(frame.translation, prevFrame.translation, dt);
angularVelocity = BurstIntegration.DifferentiateAngular(frame.rotation, prevFrame.rotation, dt);
acceleration = BurstIntegration.DifferentiateLinear(velocity, prevVelocity, dt);
angularAcceleration = BurstIntegration.DifferentiateLinear(angularVelocity, prevAngularVelocity, dt);
}
public void Transform(BurstAffineTransform transform)
{
float3x3 matrix = math.mul(new float3x3(transform.rotation), float3x3.Scale(transform.scale.xyz));
float3 xa = matrix.c0 * min.x;
float3 xb = matrix.c0 * max.x;
float3 ya = matrix.c1 * min.y;
float3 yb = matrix.c1 * max.y;
float3 za = matrix.c2 * min.z;
float3 zb = matrix.c2 * max.z;
min = new float4(math.min(xa, xb) + math.min(ya, yb) + math.min(za, zb) + transform.translation.xyz, 0);
max = new float4(math.max(xa, xb) + math.max(ya, yb) + math.max(za, zb) + transform.translation.xyz, 0);
}
private static void BIHTraverse(int particleIndex,
int colliderIndex,
float4 particlePosition,
quaternion particleOrientation,
float4 particleVelocity,
float4 particleRadii,
ref BurstAabb particleBounds,
int nodeIndex,
ref NativeArray <BIHNode> bihNodes,
ref NativeArray <Edge> edges,
ref NativeArray <float2> vertices,
ref EdgeMeshHeader header,
ref BurstAffineTransform colliderToSolver,
ref BurstColliderShape shape,
NativeQueue <BurstContact> .ParallelWriter contacts)
{
var node = bihNodes[header.firstNode + nodeIndex];
// amount by which we should inflate aabbs:
float offset = shape.contactOffset + particleRadii.x;
if (node.firstChild >= 0)
{
// visit min node:
if (particleBounds.min[node.axis] - offset <= node.min)
{
BIHTraverse(particleIndex, colliderIndex,
particlePosition, particleOrientation, particleVelocity, particleRadii, ref particleBounds,
node.firstChild, ref bihNodes, ref edges, ref vertices, ref header,
ref colliderToSolver, ref shape, contacts);
}
// visit max node:
if (particleBounds.max[node.axis] + offset >= node.max)
{
BIHTraverse(particleIndex, colliderIndex,
particlePosition, particleOrientation, particleVelocity, particleRadii, ref particleBounds,
node.firstChild + 1, ref bihNodes, ref edges, ref vertices, ref header,
ref colliderToSolver, ref shape, contacts);
}
}
else
{
// precalculate inverse of velocity vector for ray/aabb intersections:
float4 invDir = math.rcp(particleVelocity);
// contacts against all triangles:
for (int i = node.start; i < node.start + node.count; ++i)
{
Edge t = edges[header.firstEdge + i];
float4 v1 = new float4(vertices[header.firstVertex + t.i1], 0, 0) * colliderToSolver.scale;
float4 v2 = new float4(vertices[header.firstVertex + t.i2], 0, 0) * colliderToSolver.scale;
BurstAabb aabb = new BurstAabb(v1, v2, 0.01f);
aabb.Expand(new float4(offset));
// only generate a contact if the particle trajectory intersects its inflated aabb:
if (aabb.IntersectsRay(particlePosition, invDir, true))
{
float4 point = BurstMath.NearestPointOnEdge(v1, v2, particlePosition);
float4 pointToTri = particlePosition - point;
float distance = math.length(pointToTri);
if (distance > BurstMath.epsilon)
{
BurstContact c = new BurstContact()
{
entityA = particleIndex,
entityB = colliderIndex,
point = colliderToSolver.TransformPointUnscaled(point),
normal = colliderToSolver.TransformDirection(pointToTri / distance),
};
c.distance = distance - (shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, particleOrientation, particleRadii.xyz));
contacts.Enqueue(c);
}
}
}
}
}
public static void Contacts(int particleIndex,
int colliderIndex,
float4 position,
quaternion orientation,
float4 radii,
ref NativeArray <float> heightMap,
HeightFieldHeader header,
BurstAffineTransform colliderToSolver,
BurstColliderShape shape,
NativeQueue <BurstContact> .ParallelWriter contacts)
{
float4 pos = colliderToSolver.InverseTransformPoint(position);
BurstContact c = new BurstContact
{
entityA = particleIndex,
entityB = colliderIndex,
};
int resolutionU = (int)shape.center.x;
int resolutionV = (int)shape.center.y;
// calculate terrain cell size:
float cellWidth = shape.size.x / (resolutionU - 1);
float cellHeight = shape.size.z / (resolutionV - 1);
// calculate particle bounds min/max cells:
int2 min = new int2((int)math.floor((pos[0] - radii[0]) / cellWidth), (int)math.floor((pos[2] - radii[0]) / cellHeight));
int2 max = new int2((int)math.floor((pos[0] + radii[0]) / cellWidth), (int)math.floor((pos[2] + radii[0]) / cellHeight));
for (int su = min[0]; su <= max[0]; ++su)
{
if (su >= 0 && su < resolutionU - 1)
{
for (int sv = min[1]; sv <= max[1]; ++sv)
{
if (sv >= 0 && sv < resolutionV - 1)
{
// calculate neighbor sample indices:
int csu1 = math.clamp(su + 1, 0, resolutionU - 1);
int csv1 = math.clamp(sv + 1, 0, resolutionV - 1);
// sample heights:
float h1 = heightMap[header.firstSample + sv * resolutionU + su] * shape.size.y;
float h2 = heightMap[header.firstSample + sv * resolutionU + csu1] * shape.size.y;
float h3 = heightMap[header.firstSample + csv1 * resolutionU + su] * shape.size.y;
float h4 = heightMap[header.firstSample + csv1 * resolutionU + csu1] * shape.size.y;
float min_x = su * shape.size.x / (resolutionU - 1);
float max_x = csu1 * shape.size.x / (resolutionU - 1);
float min_z = sv * shape.size.z / (resolutionV - 1);
float max_z = csv1 * shape.size.z / (resolutionV - 1);
// contact with the first triangle:
float4 pointOnTri = BurstMath.NearestPointOnTri(new float4(min_x, h3, max_z, 0),
new float4(max_x, h4, max_z, 0),
new float4(min_x, h1, min_z, 0),
pos);
float4 normal = pos - pointOnTri;
float distance = math.length(normal);
if (distance > BurstMath.epsilon)
{
c.normal = normal / distance;
c.point = pointOnTri;
c.normal = colliderToSolver.TransformDirection(c.normal);
c.point = colliderToSolver.TransformPoint(c.point);
c.distance = distance - (shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz));
contacts.Enqueue(c);
}
// contact with the second triangle:
pointOnTri = BurstMath.NearestPointOnTri(new float4(min_x, h1, min_z, 0),
new float4(max_x, h4, max_z, 0),
new float4(max_x, h2, min_z, 0),
pos);
normal = pos - pointOnTri;
distance = math.length(normal);
if (distance > BurstMath.epsilon)
{
c.normal = normal / distance;
c.point = pointOnTri;
c.normal = colliderToSolver.TransformDirection(c.normal);
c.point = colliderToSolver.TransformPoint(c.point);
c.distance = distance - (shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz));
contacts.Enqueue(c);
}
}
}
}
}
}
public static void Contacts(int particleIndex,
float4 position,
quaternion orientation,
float4 radii,
int colliderIndex,
BurstAffineTransform transform,
BurstColliderShape shape,
NativeQueue <BurstContact> .ParallelWriter contacts)
{
BurstContact c = new BurstContact()
{
entityA = particleIndex,
entityB = colliderIndex,
};
float4 center = shape.center * transform.scale;
position = transform.InverseTransformPointUnscaled(position) - center;
int direction = (int)shape.size.z;
float radius = shape.size.x * math.max(transform.scale[(direction + 1) % 3], transform.scale[(direction + 2) % 3]);
float height = math.max(radius, shape.size.y * 0.5f * transform.scale[direction]);
float d = position[direction];
float4 axisProj = float4.zero;
float4 cap = float4.zero;
axisProj[direction] = d;
cap[direction] = height - radius;
float4 centerToPoint;
float centerToPointNorm;
if (d > height - radius)
{ //one cap
centerToPoint = position - cap;
centerToPointNorm = math.length(centerToPoint);
c.distance = centerToPointNorm - radius;
c.normal = (centerToPoint / (centerToPointNorm + math.FLT_MIN_NORMAL));
c.point = cap + c.normal * radius;
}
else if (d < -height + radius)
{ // other cap
centerToPoint = position + cap;
centerToPointNorm = math.length(centerToPoint);
c.distance = centerToPointNorm - radius;
c.normal = (centerToPoint / (centerToPointNorm + math.FLT_MIN_NORMAL));
c.point = -cap + c.normal * radius;
}
else
{//cylinder
centerToPoint = position - axisProj;
centerToPointNorm = math.length(centerToPoint);
c.distance = centerToPointNorm - radius;
c.normal = (centerToPoint / (centerToPointNorm + math.FLT_MIN_NORMAL));
c.point = axisProj + c.normal * radius;
}
c.point += center;
c.point = transform.TransformPointUnscaled(c.point);
c.normal = transform.TransformDirection(c.normal);
c.distance -= shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz);
contacts.Enqueue(c);
}
public void Execute(int i)
{
int simplexStart = simplexCounts.GetSimplexStartAndSize(i, out int simplexSize);
BurstAabb simplexBoundsSS = simplexBounds[i];
// get all colliders overlapped by the cell bounds, in all grid levels:
BurstAabb simplexBoundsWS = simplexBoundsSS.Transformed(solverToWorld);
NativeList <int> candidates = new NativeList <int>(Allocator.Temp);
// max size of the particle bounds in cells:
int3 maxSize = new int3(10);
bool is2D = parameters.mode == Oni.SolverParameters.Mode.Mode2D;
for (int l = 0; l < gridLevels.Length; ++l)
{
float cellSize = NativeMultilevelGrid <int> .CellSizeOfLevel(gridLevels[l]);
int3 minCell = GridHash.Quantize(simplexBoundsWS.min.xyz, cellSize);
int3 maxCell = GridHash.Quantize(simplexBoundsWS.max.xyz, cellSize);
maxCell = minCell + math.min(maxCell - minCell, maxSize);
for (int x = minCell[0]; x <= maxCell[0]; ++x)
{
for (int y = minCell[1]; y <= maxCell[1]; ++y)
{
// for 2D mode, project each cell at z == 0 and check them too. This way we ensure 2D colliders
// (which are inserted in cells with z == 0) are accounted for in the broadphase.
if (is2D)
{
if (colliderGrid.TryGetCellIndex(new int4(x, y, 0, gridLevels[l]), out int cellIndex))
{
var colliderCell = colliderGrid.usedCells[cellIndex];
candidates.AddRange(colliderCell.ContentsPointer, colliderCell.Length);
}
}
for (int z = minCell[2]; z <= maxCell[2]; ++z)
{
if (colliderGrid.TryGetCellIndex(new int4(x, y, z, gridLevels[l]), out int cellIndex))
{
var colliderCell = colliderGrid.usedCells[cellIndex];
candidates.AddRange(colliderCell.ContentsPointer, colliderCell.Length);
}
}
}
}
}
if (candidates.Length > 0)
{
// make sure each candidate collider only shows up once in the array:
NativeArray <int> uniqueCandidates = candidates.AsArray();
uniqueCandidates.Sort();
int uniqueCount = uniqueCandidates.Unique();
// iterate over candidate colliders, generating contacts for each one
for (int k = 0; k < uniqueCount; ++k)
{
int c = uniqueCandidates[k];
BurstColliderShape shape = shapes[c];
BurstAabb colliderBoundsWS = bounds[c];
// Expand bounds by rigidbody's linear velocity:
if (shape.rigidbodyIndex >= 0)
{
colliderBoundsWS.Sweep(rigidbodies[shape.rigidbodyIndex].velocity * deltaTime);
}
// Expand bounds by collision material's stick distance:
if (shape.materialIndex >= 0)
{
colliderBoundsWS.Expand(collisionMaterials[shape.materialIndex].stickDistance);
}
// check if any simplex particle and the collider have the same phase:
bool samePhase = false;
for (int j = 0; j < simplexSize; ++j)
{
samePhase |= shape.phase == (phases[simplices[simplexStart + j]] & (int)Oni.ParticleFlags.GroupMask);
}
if (!samePhase && simplexBoundsWS.IntersectsAabb(in colliderBoundsWS, is2D))
{
// generate contacts for the collider:
BurstAffineTransform colliderToSolver = worldToSolver * transforms[c];
GenerateContacts(in shape, in colliderToSolver, c, i, simplexStart, simplexSize, simplexBoundsSS);
}
}
}
}
public static float4 GetRigidbodyVelocityAtPoint(BurstRigidbody rigidbody, float4 point, float4 linearDelta, float4 angularDelta, BurstAffineTransform transform)
{
// Point is assumed to be expressed in solver space. Since rigidbodies are expressed in world space, we need to convert the
// point to world space, and convert the resulting velocity back to solver space.
return(transform.InverseTransformVector(rigidbody.GetVelocityAtPoint(transform.TransformPoint(point), linearDelta, angularDelta)));
}
public void Execute(int i)
{
var cell = particleGrid.usedCells[i];
BurstAabb cellBounds = new BurstAabb(float.MaxValue, float.MinValue);
// here we calculate cell bounds that enclose both the predicted position and the original position of all its particles,
// for accurate continuous collision detection.
for (int p = 0; p < cell.Length; ++p)
{
int pIndex = cell[p];
// get collision material stick distance:
float stickDistance = 0;
int materialIndex = particleMaterialIndices[pIndex];
if (materialIndex >= 0)
{
stickDistance = collisionMaterials[materialIndex].stickDistance;
}
cellBounds.EncapsulateParticle(positions[pIndex],
positions[pIndex] + velocities[pIndex] * deltaTime,
radii[pIndex].x + stickDistance);
}
// transform the cell bounds to world space:
cellBounds.Transform(solverToWorld);
// get all colliders overlapped by the cell bounds, in all grid levels:
NativeList <int> candidates = new NativeList <int>(Allocator.Temp);
for (int l = 0; l < gridLevels.Length; ++l)
{
GetCandidatesForBoundsAtLevel(candidates, cellBounds, gridLevels[l], is2D);
}
if (candidates.Length > 0)
{
// make sure each candidate collider only shows up once in the array:
NativeArray <int> uniqueCandidates = candidates.AsArray();
uniqueCandidates.Sort();
int uniqueCount = uniqueCandidates.Unique();
// iterate over candidate colliders, generating contacts for each one
for (int c = 0; c < uniqueCount; ++c)
{
int colliderIndex = uniqueCandidates[c];
BurstColliderShape shape = shapes[colliderIndex];
BurstAabb colliderBounds = bounds[colliderIndex];
BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
// transform collider bounds to solver space:
colliderBounds.Transform(worldToSolver);
// iterate over all particles in the cell:
for (int p = 0; p < cell.Length; ++p)
{
int particleIndex = cell[p];
// skip this pair if particle and collider have the same phase:
if (shape.phase == (phases[particleIndex] & (int)Oni.ParticleFlags.GroupMask))
{
continue;
}
// get collision material stick distance:
float stickDistance = 0;
int materialIndex = particleMaterialIndices[particleIndex];
if (materialIndex >= 0)
{
stickDistance = collisionMaterials[materialIndex].stickDistance;
}
// inflate collider bounds by particle's bounds:
BurstAabb inflatedColliderBounds = colliderBounds;
inflatedColliderBounds.Expand(radii[particleIndex].x * 1.2f + stickDistance);
float4 invDir = math.rcp(velocities[particleIndex] * deltaTime);
// We check particle trajectory ray vs inflated collider aabb
// instead of checking particle vs collider aabbs directly, as this reduces
// the amount of contacts generated for fast moving particles.
if (inflatedColliderBounds.IntersectsRay(positions[particleIndex], invDir, shape.is2D != 0))
{
// generate contacts for the collider:
GenerateContacts(shape.type,
particleIndex, colliderIndex,
positions[particleIndex], orientations[particleIndex], velocities[particleIndex], radii[particleIndex],
colliderToSolver, shape, contactsQueue, deltaTime);
}
}
}
}
}
public static void Contacts(int particleIndex,
float4 position,
quaternion orientation,
float4 radii,
int colliderIndex,
BurstAffineTransform transform,
BurstColliderShape shape,
NativeQueue <BurstContact> .ParallelWriter contacts)
{
BurstContact c = new BurstContact()
{
entityA = particleIndex,
entityB = colliderIndex,
};
float4 center = shape.center * transform.scale;
float4 size = shape.size * transform.scale * 0.5f;
position = transform.InverseTransformPointUnscaled(position) - center;
// Get minimum distance for each axis:
float4 distances = size - math.abs(position);
// if we are inside the box:
if (distances.x >= 0 && distances.y >= 0 && distances.z >= 0)
{
// find minimum distance in all three axes and the axis index:
float min = float.MaxValue;
int axis = 0;
for (int i = 0; i < 3; ++i)
{
if (distances[i] < min)
{
min = distances[i];
axis = i;
}
}
c.normal = float4.zero;
c.point = position;
c.distance = -distances[axis];
c.normal[axis] = position[axis] > 0 ? 1 : -1;
c.point[axis] = size[axis] * c.normal[axis];
}
else // we are outside the box:
{
// clamp point to be inside the box:
c.point = math.clamp(position, -size, size);
// find distance and direction to clamped point:
float4 diff = position - c.point;
c.distance = math.length(diff);
c.normal = diff / (c.distance + math.FLT_MIN_NORMAL);
}
c.point += center;
c.point = transform.TransformPointUnscaled(c.point);
c.normal = transform.TransformDirection(c.normal);
c.distance -= shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz);
contacts.Enqueue(c);
}
public BurstAffineTransform Interpolate(BurstAffineTransform other, float translationalMu, float rotationalMu, float scaleMu)
{
return(new BurstAffineTransform(math.lerp(translation, other.translation, translationalMu),
math.slerp(rotation, other.rotation, rotationalMu),
math.lerp(scale, other.scale, scaleMu)));
}
