public void Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint)
{
float4 center = shape.center * transform.scale;
point = transform.InverseTransformPointUnscaled(point) - center;
if (shape.is2D != 0)
{
point[2] = 0;
}
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]);
float4 halfVector = float4.zero;
halfVector[direction] = height - radius;
float4 centerLine = BurstMath.NearestPointOnEdge(-halfVector, halfVector, point, out float mu);
float4 centerToPoint = point - centerLine;
float distanceToCenter = math.length(centerToPoint);
float4 normal = centerToPoint / (distanceToCenter + BurstMath.epsilon);
projectedPoint.point = transform.TransformPointUnscaled(center + centerLine + normal * (radius + shape.contactOffset));
projectedPoint.normal = transform.TransformDirection(normal);
}
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);
}
public void Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint)
{
point = transform.InverseTransformPointUnscaled(point);
if (shape.is2D != 0)
{
point[2] = 0;
}
float4 nearestPoint = BurstMath.NearestPointOnTri(tri, point, out float4 bary);
float4 normal = math.normalizesafe(point - nearestPoint);
projectedPoint.point = transform.TransformPointUnscaled(nearestPoint + normal * shape.contactOffset);
projectedPoint.normal = transform.TransformDirection(normal);
}
public void Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint)
{
float4 center = shape.center * transform.scale;
float4 size = shape.size * transform.scale * 0.5f;
// clamp the point to the surface of the box:
point = transform.InverseTransformPointUnscaled(point) - center;
if (shape.is2D != 0)
{
point[2] = 0;
}
// get minimum distance for each axis:
float4 distances = size - math.abs(point);
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;
}
}
projectedPoint.normal = float4.zero;
projectedPoint.point = point;
projectedPoint.normal[axis] = point[axis] > 0 ? 1 : -1;
projectedPoint.point[axis] = size[axis] * projectedPoint.normal[axis];
}
else
{
projectedPoint.point = math.clamp(point, -size, size);
projectedPoint.normal = math.normalizesafe(point - projectedPoint.point);
}
projectedPoint.point = transform.TransformPointUnscaled(projectedPoint.point + center + projectedPoint.normal * shape.contactOffset);
projectedPoint.normal = transform.TransformDirection(projectedPoint.normal);
}
public void Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint)
{
point = transform.InverseTransformPointUnscaled(point);
if (shape.is2D != 0)
{
point[2] = 0;
}
Edge t = edges[header.firstEdge + dataOffset];
float4 v1 = (new float4(vertices[header.firstVertex + t.i1], 0) + shape.center) * transform.scale;
float4 v2 = (new float4(vertices[header.firstVertex + t.i2], 0) + shape.center) * transform.scale;
float4 nearestPoint = BurstMath.NearestPointOnEdge(v1, v2, point, out float mu);
float4 normal = math.normalizesafe(point - nearestPoint);
projectedPoint.normal = transform.TransformDirection(normal);
projectedPoint.point = transform.TransformPointUnscaled(nearestPoint + normal * shape.contactOffset);
}
public void Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint)
{
float4 center = shape.center * transform.scale;
point = transform.InverseTransformPointUnscaled(point) - center;
if (shape.is2D != 0)
{
point[2] = 0;
}
float radius = shape.size.x * math.cmax(transform.scale.xyz);
float distanceToCenter = math.length(point);
float4 normal = point / (distanceToCenter + BurstMath.epsilon);
projectedPoint.point = transform.TransformPointUnscaled(center + normal * (radius + shape.contactOffset));
projectedPoint.normal = transform.TransformDirection(normal);
}
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,
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 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);
}
