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 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);
}
private float4 GetRelativeVelocity(int particleIndex, int rigidbodyIndex, ref BurstContact contact, ref float4 angularVelocityA, ref float4 rA, ref float4 rB, bool rollingContacts)
{
// Initialize with particle linear velocity:
float4 relativeVelocity = (positions[particleIndex] - prevPositions[particleIndex]) / dt;
// Add particle angular velocity if rolling contacts are enabled:
if (rollingContacts)
{
angularVelocityA = BurstIntegration.DifferentiateAngular(orientations[particleIndex], prevOrientations[particleIndex], dt);
rA = contact.ContactPointA - prevPositions[particleIndex];
relativeVelocity += new float4(math.cross(angularVelocityA.xyz, rA.xyz), 0);
}
// Subtract rigidbody velocity:
if (rigidbodyIndex >= 0)
{
// Note: unlike rA, that is expressed in solver space, rB is expressed in world space.
rB = inertialFrame.frame.TransformPoint(contact.ContactPointB) - rigidbodies[rigidbodyIndex].com;
relativeVelocity -= BurstMath.GetRigidbodyVelocityAtPoint(rigidbodies[rigidbodyIndex], contact.ContactPointB, rigidbodyLinearDeltas[rigidbodyIndex], rigidbodyAngularDeltas[rigidbodyIndex], inertialFrame.frame);
}
return(relativeVelocity);
}
private float4 GetRelativeVelocity(int particleIndexA, int particleIndexB, ref BurstContact contact, ref float4 angularVelocityA, ref float4 angularVelocityB, ref float4 rA, ref float4 rB, bool rollingContacts)
{
// Initialize with particle linear velocity:
float4 velA = (positions[particleIndexA] - prevPositions[particleIndexA]) / dt;
float4 velB = (positions[particleIndexB] - prevPositions[particleIndexB]) / dt;
// Consider angular velocities if rolling contacts are enabled:
if (rollingContacts)
{
angularVelocityA = BurstIntegration.DifferentiateAngular(orientations[particleIndexA], prevOrientations[particleIndexA], dt);
angularVelocityB = BurstIntegration.DifferentiateAngular(orientations[particleIndexB], prevOrientations[particleIndexB], dt);
rA = contact.ContactPointA - prevPositions[particleIndexA];
rB = contact.ContactPointB - prevPositions[particleIndexB];
velA += new float4(math.cross(angularVelocityA.xyz, rA.xyz), 0);
velB += new float4(math.cross(angularVelocityB.xyz, rB.xyz), 0);
}
return(velA - velB);
}
private float4 GetRelativeVelocity(int particleIndex, int rigidbodyIndex, ref BurstContact contact)
{
// Initialize with particle linear velocity:
float4 relativeVelocity = (positions[particleIndex] - prevPositions[particleIndex]) / dt;
// As we do not consider true ellipses for collision detection, particle contact points are never off-axis.
// So particle angular velocity does not contribute to normal impulses, and we can skip it.
// Subtract rigidbody velocity:
if (rigidbodyIndex >= 0)
{
relativeVelocity -= BurstMath.GetRigidbodyVelocityAtPoint(rigidbodies[rigidbodyIndex], contact.ContactPointB, rigidbodyLinearDeltas[rigidbodyIndex], rigidbodyAngularDeltas[rigidbodyIndex], inertialFrame.frame);
}
return(relativeVelocity);
}
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);
}
