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 void Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint)
{
switch (simplexSize)
{
case 1:
{
float4 p1 = positions[simplices[simplexStart]];
projectedPoint.bary = new float4(1, 0, 0, 0);
projectedPoint.point = p1;
}
break;
case 2:
{
float4 p1 = positions[simplices[simplexStart]];
float4 p2 = positions[simplices[simplexStart + 1]];
BurstMath.NearestPointOnEdge(p1, p2, point, out float mu);
projectedPoint.bary = new float4(1 - mu, mu, 0, 0);
projectedPoint.point = p1 * projectedPoint.bary[0] + p2 * projectedPoint.bary[1];
} break;
case 3:
projectedPoint.point = BurstMath.NearestPointOnTri(tri, point, out projectedPoint.bary);
break;
}
projectedPoint.normal = math.normalizesafe(point - projectedPoint.point);
/*float radius1 = radii[simplices[simplexStart]].x;
* float radius2 = radii[simplices[simplexStart+1]].x;
*
* float invLen2 = 1.0f / math.lengthsq(p1 - p2);
* float dl = (radius1 - radius2) * invLen2;
* float sl = math.sqrt(1.0f / invLen2 - math.pow(radius1 - radius2, 2)) * math.sqrt(invLen2);
* float adj_radii1 = radius1 * sl;
* float adj_radii2 = radius2 * sl;
*
* float trange1 = radius1 * dl;
* float trange2 = 1 + radius2 * dl;
*
* float adj_t = (mu - trange1) / (trange2 - trange1);
* float radius = adj_radii1 + adj_t * (adj_radii2 - adj_radii1);
*
* float4 centerToPoint = point - centerLine;
* float4 normal = centerToPoint / (math.length(centerToPoint) + BurstMath.epsilon);
*
* projectedPoint.point = centerLine + normal * radius;
* projectedPoint.normal = 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);
}
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);
}
}
}
}
}