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 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 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 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 Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint)
{
point = transform.InverseTransformPoint(point);
if (shape.is2D != 0)
{
point[2] = 0;
}
var header = distanceFieldHeaders[shape.dataIndex];
float4 sample = DFTraverse(point, 0, in header, in dfNodes);
float4 normal = new float4(math.normalize(sample.xyz), 0);
projectedPoint.point = transform.TransformPoint(point - normal * (sample[3] - shape.contactOffset));
projectedPoint.normal = transform.TransformDirection(normal);
}
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 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)));
}
