public void Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint)
{
point = transform.InverseTransformPoint(point);
float4 nearestPoint = BurstMath.NearestPointOnTri(tri, point, out float4 bary);
float4 normal = math.normalizesafe(point - nearestPoint);
// flip the contact normal if it points below ground:
BurstMath.OneSidedNormal(triNormal, ref normal);
projectedPoint.point = transform.TransformPoint(nearestPoint + normal * 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;
}
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);
}
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 <Triangle> triangles,
ref NativeArray <float3> vertices,
ref TriangleMeshHeader 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 triangles, 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 triangles, 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)
{
Triangle t = triangles[header.firstTriangle + i];
float4 v1 = new float4(vertices[header.firstVertex + t.i1], 0) * colliderToSolver.scale;
float4 v2 = new float4(vertices[header.firstVertex + t.i2], 0) * colliderToSolver.scale;
float4 v3 = new float4(vertices[header.firstVertex + t.i3], 0) * colliderToSolver.scale;
BurstAabb aabb = new BurstAabb(v1, v2, v3, 0.01f);
aabb.Expand(new float4(offset));
// only generate a contact if the particle trajectory intersects its inflated aabb:
if (aabb.IntersectsRay(particlePosition, invDir))
{
float4 point = BurstMath.NearestPointOnTri(v1, v2, v3, 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);
}
}
}
}
}
}