升级obi
This commit is contained in:
@@ -1,17 +1,15 @@
|
||||
#if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
|
||||
using Unity.Burst;
|
||||
using Unity.Collections;
|
||||
using Unity.Jobs;
|
||||
using Unity.Mathematics;
|
||||
|
||||
namespace Obi
|
||||
{
|
||||
public struct BurstDistanceField : BurstLocalOptimization.IDistanceFunction, IBurstCollider
|
||||
public struct BurstDistanceField : BurstLocalOptimization.IDistanceFunction
|
||||
{
|
||||
public BurstColliderShape shape;
|
||||
public BurstAffineTransform colliderToSolver;
|
||||
public BurstAffineTransform solverToWorld;
|
||||
|
||||
public float dt;
|
||||
public float collisionMargin;
|
||||
|
||||
public NativeArray<DistanceFieldHeader> distanceFieldHeaders;
|
||||
public NativeArray<BurstDFNode> dfNodes;
|
||||
@@ -20,47 +18,143 @@ namespace Obi
|
||||
{
|
||||
point = colliderToSolver.InverseTransformPoint(point);
|
||||
|
||||
if (shape.is2D != 0)
|
||||
if (shape.is2D)
|
||||
point[2] = 0;
|
||||
|
||||
var header = distanceFieldHeaders[shape.dataIndex];
|
||||
float4 sample = DFTraverse(point, 0, in header, in dfNodes);
|
||||
float4 sample = DFTraverse(point, in header);
|
||||
float4 normal = new float4(math.normalize(sample.xyz), 0);
|
||||
|
||||
projectedPoint.point = colliderToSolver.TransformPoint(point - normal * (sample[3] - shape.contactOffset));
|
||||
projectedPoint.normal = colliderToSolver.TransformDirection(normal);
|
||||
}
|
||||
|
||||
public void Contacts(int colliderIndex,
|
||||
int rigidbodyIndex,
|
||||
NativeArray<BurstRigidbody> rigidbodies,
|
||||
|
||||
NativeArray<float4> positions,
|
||||
NativeArray<quaternion> orientations,
|
||||
NativeArray<float4> velocities,
|
||||
NativeArray<float4> radii,
|
||||
|
||||
NativeArray<int> simplices,
|
||||
in BurstAabb simplexBounds,
|
||||
int simplexIndex,
|
||||
int simplexStart,
|
||||
int simplexSize,
|
||||
|
||||
NativeQueue<BurstContact>.ParallelWriter contacts,
|
||||
int optimizationIterations,
|
||||
float optimizationTolerance)
|
||||
private float4 DFTraverse(float4 particlePosition, in DistanceFieldHeader header)
|
||||
{
|
||||
if (shape.dataIndex < 0) return;
|
||||
var stack = new NativeArray<int>(12, Allocator.Temp);
|
||||
int stackTop = 0;
|
||||
|
||||
stack[stackTop++] = 0;
|
||||
|
||||
while (stackTop > 0)
|
||||
{
|
||||
int nodeIndex = stack[--stackTop];
|
||||
var node = dfNodes[header.firstNode + nodeIndex];
|
||||
|
||||
// if the child node exists, recurse down the df octree:
|
||||
if (node.firstChild >= 0)
|
||||
stack[stackTop++] = node.firstChild + node.GetOctant(particlePosition);
|
||||
else
|
||||
return node.SampleWithGradient(particlePosition);
|
||||
}
|
||||
return float4.zero;
|
||||
}
|
||||
|
||||
public static JobHandle GenerateContacts(ObiColliderWorld world,
|
||||
BurstSolverImpl solver,
|
||||
NativeList<Oni.ContactPair> contactPairs,
|
||||
NativeQueue<BurstContact> contactQueue,
|
||||
NativeArray<int> contactOffsetsPerType,
|
||||
float deltaTime,
|
||||
JobHandle inputDeps)
|
||||
{
|
||||
int pairCount = contactOffsetsPerType[(int)Oni.ShapeType.SignedDistanceField + 1] - contactOffsetsPerType[(int)Oni.ShapeType.SignedDistanceField];
|
||||
if (pairCount == 0) return inputDeps;
|
||||
|
||||
var job = new GenerateDistanceFieldContactsJob
|
||||
{
|
||||
contactPairs = contactPairs,
|
||||
|
||||
positions = solver.positions,
|
||||
orientations = solver.orientations,
|
||||
velocities = solver.velocities,
|
||||
invMasses = solver.invMasses,
|
||||
radii = solver.principalRadii,
|
||||
|
||||
simplices = solver.simplices,
|
||||
simplexCounts = solver.simplexCounts,
|
||||
|
||||
transforms = world.colliderTransforms.AsNativeArray<BurstAffineTransform>(),
|
||||
shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
|
||||
rigidbodies = world.rigidbodies.AsNativeArray<BurstRigidbody>(),
|
||||
|
||||
distanceFieldHeaders = world.distanceFieldContainer.headers.AsNativeArray<DistanceFieldHeader>(),
|
||||
distanceFieldNodes = world.distanceFieldContainer.dfNodes.AsNativeArray<BurstDFNode>(),
|
||||
|
||||
contactsQueue = contactQueue.AsParallelWriter(),
|
||||
|
||||
solverToWorld = solver.inertialFrame,
|
||||
worldToSolver = solver.worldToSolver,
|
||||
deltaTime = deltaTime,
|
||||
parameters = solver.abstraction.parameters,
|
||||
firstPair = contactOffsetsPerType[(int)Oni.ShapeType.SignedDistanceField]
|
||||
};
|
||||
|
||||
inputDeps = job.Schedule(pairCount, 1, inputDeps);
|
||||
return inputDeps;
|
||||
}
|
||||
}
|
||||
|
||||
[BurstCompile]
|
||||
struct GenerateDistanceFieldContactsJob : IJobParallelFor
|
||||
{
|
||||
[ReadOnly] public NativeList<Oni.ContactPair> contactPairs;
|
||||
|
||||
// particle arrays:
|
||||
[ReadOnly] public NativeArray<float4> velocities;
|
||||
[ReadOnly] public NativeArray<float4> positions;
|
||||
[ReadOnly] public NativeArray<quaternion> orientations;
|
||||
[ReadOnly] public NativeArray<float> invMasses;
|
||||
[ReadOnly] public NativeArray<float4> radii;
|
||||
|
||||
// simplex arrays:
|
||||
[ReadOnly] public NativeArray<int> simplices;
|
||||
[ReadOnly] public SimplexCounts simplexCounts;
|
||||
|
||||
// collider arrays:
|
||||
[ReadOnly] public NativeArray<BurstAffineTransform> transforms;
|
||||
[ReadOnly] public NativeArray<BurstColliderShape> shapes;
|
||||
[ReadOnly] public NativeArray<BurstRigidbody> rigidbodies;
|
||||
|
||||
// distance field data:
|
||||
[ReadOnly] public NativeArray<DistanceFieldHeader> distanceFieldHeaders;
|
||||
[ReadOnly] public NativeArray<BurstDFNode> distanceFieldNodes;
|
||||
|
||||
[WriteOnly]
|
||||
[NativeDisableParallelForRestriction]
|
||||
public NativeQueue<BurstContact>.ParallelWriter contactsQueue;
|
||||
|
||||
// auxiliar data:
|
||||
[ReadOnly] public int firstPair;
|
||||
[ReadOnly] public BurstInertialFrame solverToWorld;
|
||||
[ReadOnly] public BurstAffineTransform worldToSolver;
|
||||
[ReadOnly] public float deltaTime;
|
||||
[ReadOnly] public Oni.SolverParameters parameters;
|
||||
|
||||
public void Execute(int i)
|
||||
{
|
||||
|
||||
int simplexIndex = contactPairs[firstPair + i].bodyA;
|
||||
int colliderIndex = contactPairs[firstPair + i].bodyB;
|
||||
int rigidbodyIndex = shapes[colliderIndex].rigidbodyIndex;
|
||||
|
||||
if (shapes[colliderIndex].dataIndex < 0) return;
|
||||
|
||||
int simplexStart = simplexCounts.GetSimplexStartAndSize(simplexIndex, out int simplexSize);
|
||||
BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
|
||||
|
||||
BurstDistanceField dfShape = new BurstDistanceField()
|
||||
{
|
||||
colliderToSolver = colliderToSolver,
|
||||
shape = shapes[colliderIndex],
|
||||
distanceFieldHeaders = distanceFieldHeaders,
|
||||
dfNodes = distanceFieldNodes
|
||||
};
|
||||
|
||||
|
||||
var co = new BurstContact() { bodyA = simplexIndex, bodyB = colliderIndex };
|
||||
float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
|
||||
|
||||
var colliderPoint = BurstLocalOptimization.Optimize<BurstDistanceField>(ref this, positions, orientations, radii, simplices, simplexStart, simplexSize,
|
||||
ref simplexBary, out float4 simplexPoint, optimizationIterations, optimizationTolerance);
|
||||
|
||||
co.pointB = colliderPoint.point;
|
||||
co.normal = colliderPoint.normal;
|
||||
co.pointA = simplexBary;
|
||||
var colliderPoint = BurstLocalOptimization.Optimize(ref dfShape, positions, orientations, radii, simplices, simplexStart, simplexSize,
|
||||
ref simplexBary, out float4 simplexPoint, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
|
||||
|
||||
float4 velocity = float4.zero;
|
||||
float simplexRadius = 0;
|
||||
@@ -76,31 +170,18 @@ namespace Obi
|
||||
rbVelocity = BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, colliderPoint.point, rigidbodies, solverToWorld);
|
||||
|
||||
float dAB = math.dot(simplexPoint - colliderPoint.point, colliderPoint.normal);
|
||||
float vel = math.dot(velocity - rbVelocity, colliderPoint.normal);
|
||||
float vel = math.dot(velocity - rbVelocity, colliderPoint.normal);
|
||||
|
||||
if (vel * dt + dAB <= simplexRadius + shape.contactOffset + collisionMargin)
|
||||
contacts.Enqueue(co);
|
||||
//if (vel * deltaTime + dAB <= simplexRadius + shapes[colliderIndex].contactOffset + parameters.collisionMargin)
|
||||
contactsQueue.Enqueue(new BurstContact
|
||||
{
|
||||
bodyA = simplexIndex,
|
||||
bodyB = colliderIndex,
|
||||
pointA = simplexBary,
|
||||
pointB = colliderPoint.point,
|
||||
normal = colliderPoint.normal * dfShape.shape.sign
|
||||
});
|
||||
}
|
||||
|
||||
private static float4 DFTraverse(float4 particlePosition,
|
||||
int nodeIndex,
|
||||
in DistanceFieldHeader header,
|
||||
in NativeArray<BurstDFNode> dfNodes)
|
||||
{
|
||||
var node = dfNodes[header.firstNode + nodeIndex];
|
||||
|
||||
// if the child node exists, recurse down the df octree:
|
||||
if (node.firstChild >= 0)
|
||||
{
|
||||
int octant = node.GetOctant(particlePosition);
|
||||
return DFTraverse(particlePosition, node.firstChild + octant, in header, in dfNodes);
|
||||
}
|
||||
else
|
||||
{
|
||||
return node.SampleWithGradient(particlePosition);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user