去掉obi，使用自写绳索
This commit is contained in:
@@ -1,155 +0,0 @@
|
||||
#if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
|
||||
using Unity.Burst;
|
||||
using Unity.Collections;
|
||||
using Unity.Jobs;
|
||||
using Unity.Mathematics;
|
||||
|
||||
namespace Obi
|
||||
{
|
||||
public struct BurstBox : BurstLocalOptimization.IDistanceFunction
|
||||
{
|
||||
public BurstColliderShape shape;
|
||||
public BurstAffineTransform colliderToSolver;
|
||||
|
||||
public void Evaluate(float4 point, float4 radii, quaternion orientation, ref BurstLocalOptimization.SurfacePoint projectedPoint)
|
||||
{
|
||||
float4 center = shape.center * colliderToSolver.scale;
|
||||
float4 size = shape.size * colliderToSolver.scale * 0.5f;
|
||||
|
||||
// clamp the point to the surface of the box:
|
||||
point = colliderToSolver.InverseTransformPointUnscaled(point) - center;
|
||||
|
||||
if (shape.is2D)
|
||||
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 = colliderToSolver.TransformPointUnscaled(projectedPoint.point + center + projectedPoint.normal * shape.contactOffset);
|
||||
projectedPoint.normal = colliderToSolver.TransformDirection(projectedPoint.normal);
|
||||
}
|
||||
|
||||
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.Box + 1] - contactOffsetsPerType[(int)Oni.ShapeType.Box];
|
||||
if (pairCount == 0) return inputDeps;
|
||||
|
||||
var job = new GenerateBoxContactsJob
|
||||
{
|
||||
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>(),
|
||||
|
||||
contactsQueue = contactQueue.AsParallelWriter(),
|
||||
|
||||
worldToSolver = solver.worldToSolver,
|
||||
deltaTime = deltaTime,
|
||||
parameters = solver.abstraction.parameters,
|
||||
firstPair = contactOffsetsPerType[(int)Oni.ShapeType.Box]
|
||||
};
|
||||
|
||||
inputDeps = job.Schedule(pairCount, 8, inputDeps);
|
||||
return inputDeps;
|
||||
}
|
||||
}
|
||||
|
||||
[BurstCompile]
|
||||
struct GenerateBoxContactsJob : 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;
|
||||
|
||||
[WriteOnly]
|
||||
[NativeDisableParallelForRestriction]
|
||||
public NativeQueue<BurstContact>.ParallelWriter contactsQueue;
|
||||
|
||||
// auxiliar data:
|
||||
[ReadOnly] public int firstPair;
|
||||
[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 simplexStart = simplexCounts.GetSimplexStartAndSize(simplexIndex, out int simplexSize);
|
||||
|
||||
BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
|
||||
|
||||
BurstBox shape = new BurstBox { colliderToSolver = colliderToSolver, shape = shapes[colliderIndex] };
|
||||
|
||||
float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
|
||||
var colliderPoint = BurstLocalOptimization.Optimize(ref shape, positions, orientations, radii, simplices, simplexStart, simplexSize,
|
||||
ref simplexBary, out _, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
|
||||
|
||||
contactsQueue.Enqueue(new BurstContact
|
||||
{
|
||||
bodyA = simplexIndex,
|
||||
bodyB = colliderIndex,
|
||||
pointA = simplexBary,
|
||||
pointB = colliderPoint.point,
|
||||
normal = colliderPoint.normal * shape.shape.sign
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
@@ -1,11 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: f09926d9bc8604475a3bce5e593165b4
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,136 +0,0 @@
|
||||
#if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
|
||||
using Unity.Burst;
|
||||
using Unity.Collections;
|
||||
using Unity.Jobs;
|
||||
using Unity.Mathematics;
|
||||
|
||||
namespace Obi
|
||||
{
|
||||
public struct BurstCapsule : BurstLocalOptimization.IDistanceFunction
|
||||
{
|
||||
public BurstColliderShape shape;
|
||||
public BurstAffineTransform colliderToSolver;
|
||||
|
||||
public void Evaluate(float4 point, float4 radii, quaternion orientation, ref BurstLocalOptimization.SurfacePoint projectedPoint)
|
||||
{
|
||||
float4 center = shape.center * colliderToSolver.scale;
|
||||
point = colliderToSolver.InverseTransformPointUnscaled(point) - center;
|
||||
|
||||
if (shape.is2D)
|
||||
point[2] = 0;
|
||||
|
||||
int direction = (int)shape.size.z;
|
||||
float radius = shape.size.x * math.max(colliderToSolver.scale[(direction + 1) % 3],
|
||||
colliderToSolver.scale[(direction + 2) % 3]);
|
||||
|
||||
float height = math.max(radius, shape.size.y * 0.5f * colliderToSolver.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 = colliderToSolver.TransformPointUnscaled(center + centerLine + normal * (radius + shape.contactOffset));
|
||||
projectedPoint.normal = colliderToSolver.TransformDirection(normal);
|
||||
}
|
||||
|
||||
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.Capsule + 1] - contactOffsetsPerType[(int)Oni.ShapeType.Capsule];
|
||||
if (pairCount == 0) return inputDeps;
|
||||
|
||||
var job = new GenerateCapsuleContactsJob
|
||||
{
|
||||
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>(),
|
||||
|
||||
contactsQueue = contactQueue.AsParallelWriter(),
|
||||
|
||||
worldToSolver = solver.worldToSolver,
|
||||
deltaTime = deltaTime,
|
||||
parameters = solver.abstraction.parameters,
|
||||
firstPair = contactOffsetsPerType[(int)Oni.ShapeType.Capsule]
|
||||
};
|
||||
|
||||
inputDeps = job.Schedule(pairCount, 8, inputDeps);
|
||||
return inputDeps;
|
||||
}
|
||||
}
|
||||
|
||||
[BurstCompile]
|
||||
struct GenerateCapsuleContactsJob : 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;
|
||||
|
||||
[WriteOnly]
|
||||
[NativeDisableParallelForRestriction]
|
||||
public NativeQueue<BurstContact>.ParallelWriter contactsQueue;
|
||||
|
||||
// auxiliar data:
|
||||
[ReadOnly] public int firstPair;
|
||||
[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 simplexStart = simplexCounts.GetSimplexStartAndSize(simplexIndex, out int simplexSize);
|
||||
|
||||
BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
|
||||
|
||||
BurstCapsule shape = new BurstCapsule { colliderToSolver = colliderToSolver, shape = shapes[colliderIndex] };
|
||||
|
||||
float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
|
||||
var colliderPoint = BurstLocalOptimization.Optimize(ref shape, positions, orientations, radii, simplices, simplexStart, simplexSize,
|
||||
ref simplexBary, out _, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
|
||||
|
||||
contactsQueue.Enqueue(new BurstContact
|
||||
{
|
||||
bodyA = simplexIndex,
|
||||
bodyB = colliderIndex,
|
||||
pointA = simplexBary,
|
||||
pointB = colliderPoint.point,
|
||||
normal = colliderPoint.normal * shape.shape.sign
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
@@ -1,11 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 30a6f80040171467eb7e40b869d0aa44
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,41 +0,0 @@
|
||||
#if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
|
||||
using Unity.Mathematics;
|
||||
|
||||
namespace Obi
|
||||
{
|
||||
|
||||
public struct BurstColliderShape
|
||||
{
|
||||
public float4 center;
|
||||
public float4 size; /**< box: size of the box in each axis.
|
||||
sphere: radius of sphere (x,y,z),
|
||||
capsule: radius (x), height(y), direction (z, can be 0, 1 or 2).
|
||||
heightmap: width (x axis), height (y axis) and depth (z axis) in world units.*/
|
||||
|
||||
public ColliderShape.ShapeType type;
|
||||
public float contactOffset;
|
||||
public int dataIndex; // index of the associated collider data in the collision world.
|
||||
public int rigidbodyIndex; // index of the associated rigidbody in the collision world.
|
||||
public int materialIndex; // index of the associated material in the collision world.
|
||||
public int forceZoneIndex; // index of the associated force zone in the collision world.
|
||||
public int filter;
|
||||
public int flags; // first bit whether the collider is 2D (1) or 3D (0), second bit whether it's a trigger (1) or regular collider (0),
|
||||
// third bit determines whether shape is inverted or not.
|
||||
|
||||
public bool is2D
|
||||
{
|
||||
get => (flags & 1) != 0;
|
||||
set => flags |= value ? 1 : 0;
|
||||
}
|
||||
public bool isTrigger
|
||||
{
|
||||
get => (flags & 1 << 1) != 0 || forceZoneIndex >= 0;
|
||||
set => flags |= value ? 1 << 1 : 0;
|
||||
}
|
||||
public float sign
|
||||
{
|
||||
get => (flags & 1 << 2) != 0 ? -1 : 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
@@ -1,11 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 707535693276c450e8ba4504defa09eb
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,632 +0,0 @@
|
||||
#if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
|
||||
using UnityEngine;
|
||||
using Unity.Collections;
|
||||
using Unity.Jobs;
|
||||
using Unity.Mathematics;
|
||||
using Unity.Burst;
|
||||
using System.Threading;
|
||||
using Unity.Collections.LowLevel.Unsafe;
|
||||
|
||||
namespace Obi
|
||||
{
|
||||
|
||||
|
||||
public class BurstColliderWorld : MonoBehaviour, IColliderWorldImpl
|
||||
{
|
||||
struct MovingCollider
|
||||
{
|
||||
public BurstCellSpan oldSpan;
|
||||
public BurstCellSpan newSpan;
|
||||
public int entity;
|
||||
}
|
||||
|
||||
public int referenceCount { get; private set; } = 0;
|
||||
public int colliderCount { get; private set; } = 0;
|
||||
|
||||
private NativeMultilevelGrid<int> grid;
|
||||
private NativeQueue<MovingCollider> movingColliders;
|
||||
|
||||
private NativeArray<int> colliderTypeCounts;
|
||||
private NativeQueue<Oni.ContactPair> contactPairQueue;
|
||||
public NativeList<Oni.ContactPair> contactPairs;
|
||||
public NativeArray<int> contactOffsetsPerType;
|
||||
|
||||
public NativeQueue<BurstContact> colliderContactQueue;
|
||||
|
||||
public ObiNativeCellSpanList cellSpans;
|
||||
|
||||
public void Awake()
|
||||
{
|
||||
this.grid = new NativeMultilevelGrid<int>(1000, Allocator.Persistent);
|
||||
this.movingColliders = new NativeQueue<MovingCollider>(Allocator.Persistent);
|
||||
this.colliderContactQueue = new NativeQueue<BurstContact>(Allocator.Persistent);
|
||||
|
||||
this.contactPairQueue = new NativeQueue<Oni.ContactPair>(Allocator.Persistent);
|
||||
this.colliderTypeCounts = new NativeArray<int>(Oni.ColliderShapeTypeCount, Allocator.Persistent);
|
||||
this.contactOffsetsPerType = new NativeArray<int>(Oni.ColliderShapeTypeCount + 1, Allocator.Persistent);
|
||||
this.contactPairs = new NativeList<Oni.ContactPair>(Allocator.Persistent);
|
||||
|
||||
this.cellSpans = new ObiNativeCellSpanList();
|
||||
|
||||
ObiColliderWorld.GetInstance().RegisterImplementation(this);
|
||||
}
|
||||
|
||||
public void OnDestroy()
|
||||
{
|
||||
ObiColliderWorld.GetInstance().UnregisterImplementation(this);
|
||||
|
||||
grid.Dispose();
|
||||
movingColliders.Dispose();
|
||||
|
||||
colliderTypeCounts.Dispose();
|
||||
contactPairQueue.Dispose();
|
||||
contactPairs.Dispose();
|
||||
contactOffsetsPerType.Dispose();
|
||||
|
||||
colliderContactQueue.Dispose();
|
||||
|
||||
cellSpans.Dispose();
|
||||
}
|
||||
|
||||
public void IncreaseReferenceCount()
|
||||
{
|
||||
referenceCount++;
|
||||
}
|
||||
public void DecreaseReferenceCount()
|
||||
{
|
||||
if (--referenceCount <= 0 && gameObject != null)
|
||||
DestroyImmediate(gameObject);
|
||||
}
|
||||
|
||||
public void SetColliders(ObiNativeColliderShapeList shapes, ObiNativeAabbList bounds, ObiNativeAffineTransformList transforms)
|
||||
{
|
||||
colliderCount = shapes.count;
|
||||
|
||||
// insert new empty cellspans at the end if needed:
|
||||
while (colliderCount > cellSpans.count)
|
||||
cellSpans.Add(new CellSpan(new VInt4(10000), new VInt4(10000)));
|
||||
}
|
||||
|
||||
public void SetRigidbodies(ObiNativeRigidbodyList rigidbody)
|
||||
{
|
||||
}
|
||||
|
||||
public void SetForceZones(ObiNativeForceZoneList rigidbody)
|
||||
{
|
||||
}
|
||||
|
||||
public void SetCollisionMaterials(ObiNativeCollisionMaterialList materials)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
public void SetTriangleMeshData(ObiNativeTriangleMeshHeaderList headers, ObiNativeBIHNodeList nodes, ObiNativeTriangleList triangles, ObiNativeVector3List vertices)
|
||||
{
|
||||
}
|
||||
|
||||
public void SetEdgeMeshData(ObiNativeEdgeMeshHeaderList headers, ObiNativeBIHNodeList nodes, ObiNativeEdgeList edges, ObiNativeVector2List vertices)
|
||||
{
|
||||
}
|
||||
|
||||
public void SetDistanceFieldData(ObiNativeDistanceFieldHeaderList headers, ObiNativeDFNodeList nodes) { }
|
||||
public void SetHeightFieldData(ObiNativeHeightFieldHeaderList headers, ObiNativeFloatList samples) { }
|
||||
|
||||
public void UpdateWorld(float deltaTime)
|
||||
{
|
||||
var world = ObiColliderWorld.GetInstance();
|
||||
|
||||
var identifyMoving = new IdentifyMovingColliders
|
||||
{
|
||||
movingColliders = movingColliders.AsParallelWriter(),
|
||||
shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(cellSpans.count),
|
||||
rigidbodies = world.rigidbodies.AsNativeArray<BurstRigidbody>(),
|
||||
collisionMaterials = world.collisionMaterials.AsNativeArray<BurstCollisionMaterial>(),
|
||||
bounds = world.colliderAabbs.AsNativeArray<BurstAabb>(cellSpans.count),
|
||||
cellIndices = cellSpans.AsNativeArray<BurstCellSpan>(),
|
||||
colliderCount = colliderCount,
|
||||
dt = deltaTime
|
||||
};
|
||||
JobHandle movingHandle = identifyMoving.Schedule(cellSpans.count, 128);
|
||||
|
||||
var updateMoving = new UpdateMovingColliders
|
||||
{
|
||||
movingColliders = movingColliders,
|
||||
grid = grid,
|
||||
colliderCount = colliderCount
|
||||
};
|
||||
|
||||
updateMoving.Schedule(movingHandle).Complete();
|
||||
|
||||
// remove tail from the current spans array:
|
||||
if (colliderCount < cellSpans.count)
|
||||
cellSpans.count -= cellSpans.count - colliderCount;
|
||||
}
|
||||
|
||||
[BurstCompile]
|
||||
struct IdentifyMovingColliders : IJobParallelFor
|
||||
{
|
||||
[WriteOnly]
|
||||
[NativeDisableParallelForRestriction]
|
||||
public NativeQueue<MovingCollider>.ParallelWriter movingColliders;
|
||||
|
||||
[ReadOnly] public NativeArray<BurstColliderShape> shapes;
|
||||
[ReadOnly] public NativeArray<BurstRigidbody> rigidbodies;
|
||||
[ReadOnly] public NativeArray<BurstCollisionMaterial> collisionMaterials;
|
||||
public NativeArray<BurstAabb> bounds;
|
||||
|
||||
public NativeArray<BurstCellSpan> cellIndices;
|
||||
[ReadOnly] public int colliderCount;
|
||||
[ReadOnly] public float dt;
|
||||
|
||||
// Iterate over all colliders and store those whose cell span has changed.
|
||||
public void Execute(int i)
|
||||
{
|
||||
BurstAabb velocityBounds = bounds[i];
|
||||
|
||||
int rb = shapes[i].rigidbodyIndex;
|
||||
|
||||
// Expand bounds by rigidbody's linear velocity
|
||||
// (check against out of bounds rigidbody access, can happen when a destroyed collider references a rigidbody that has just been destroyed too)
|
||||
if (rb >= 0 && rb < rigidbodies.Length)
|
||||
velocityBounds.Sweep(rigidbodies[rb].velocity * dt);
|
||||
|
||||
// Expand bounds by collision material's stick distance:
|
||||
if (shapes[i].materialIndex >= 0)
|
||||
velocityBounds.Expand(collisionMaterials[shapes[i].materialIndex].stickDistance);
|
||||
|
||||
float size = velocityBounds.AverageAxisLength();
|
||||
int level = NativeMultilevelGrid<int>.GridLevelForSize(size);
|
||||
float cellSize = NativeMultilevelGrid<int>.CellSizeOfLevel(level);
|
||||
|
||||
// get new collider bounds cell coordinates:
|
||||
BurstCellSpan newSpan = new BurstCellSpan(new int4(GridHash.Quantize(velocityBounds.min.xyz, cellSize), level),
|
||||
new int4(GridHash.Quantize(velocityBounds.max.xyz, cellSize), level));
|
||||
|
||||
// if the collider is 2D, project it to the z = 0 cells.
|
||||
if (shapes[i].is2D)
|
||||
{
|
||||
newSpan.min[2] = 0;
|
||||
newSpan.max[2] = 0;
|
||||
}
|
||||
|
||||
// if the collider is at the tail (removed), we will only remove it from its current cellspan.
|
||||
// if the new cellspan and the current one are different, we must remove it from its current cellspan and add it to its new one.
|
||||
if (i >= colliderCount || cellIndices[i] != newSpan)
|
||||
{
|
||||
// Add the collider to the list of moving colliders:
|
||||
movingColliders.Enqueue(new MovingCollider
|
||||
{
|
||||
oldSpan = cellIndices[i],
|
||||
newSpan = newSpan,
|
||||
entity = i
|
||||
});
|
||||
|
||||
// Update previous coords:
|
||||
cellIndices[i] = newSpan;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
[BurstCompile]
|
||||
struct UpdateMovingColliders : IJob
|
||||
{
|
||||
public NativeQueue<MovingCollider> movingColliders;
|
||||
public NativeMultilevelGrid<int> grid;
|
||||
[ReadOnly] public int colliderCount;
|
||||
|
||||
public void Execute()
|
||||
{
|
||||
while (movingColliders.Count > 0)
|
||||
{
|
||||
MovingCollider movingCollider = movingColliders.Dequeue();
|
||||
|
||||
// remove from old cells:
|
||||
grid.RemoveFromCells(movingCollider.oldSpan, movingCollider.entity);
|
||||
|
||||
// insert in new cells, as long as the index is below the amount of colliders.
|
||||
// otherwise, the collider is at the "tail" and there's no need to add it back.
|
||||
if (movingCollider.entity < colliderCount)
|
||||
grid.AddToCells(movingCollider.newSpan, movingCollider.entity);
|
||||
}
|
||||
|
||||
// remove all empty cells from the grid:
|
||||
grid.RemoveEmpty();
|
||||
}
|
||||
}
|
||||
|
||||
[BurstCompile]
|
||||
unsafe struct GenerateContactsJob : IJobParallelFor
|
||||
{
|
||||
//collider grid:
|
||||
[ReadOnly] public NativeMultilevelGrid<int> colliderGrid;
|
||||
|
||||
[DeallocateOnJobCompletion]
|
||||
[ReadOnly] public NativeArray<int> gridLevels;
|
||||
|
||||
// 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;
|
||||
[ReadOnly] public NativeArray<int> filters;
|
||||
[ReadOnly] public NativeArray<int> particleMaterialIndices;
|
||||
|
||||
// simplex arrays:
|
||||
[ReadOnly] public NativeArray<int> simplices;
|
||||
[ReadOnly] public SimplexCounts simplexCounts;
|
||||
[ReadOnly] public NativeArray<BurstAabb> simplexBounds;
|
||||
|
||||
// collider arrays:
|
||||
[ReadOnly] public NativeArray<BurstAffineTransform> transforms;
|
||||
[ReadOnly] public NativeArray<BurstColliderShape> shapes;
|
||||
[ReadOnly] public NativeArray<BurstCollisionMaterial> collisionMaterials;
|
||||
[ReadOnly] public NativeArray<BurstRigidbody> rigidbodies;
|
||||
[ReadOnly] public NativeArray<BurstAabb> bounds;
|
||||
|
||||
[WriteOnly]
|
||||
[NativeDisableParallelForRestriction]
|
||||
public NativeQueue<Oni.ContactPair>.ParallelWriter contactPairQueue;
|
||||
|
||||
[NativeDisableParallelForRestriction]
|
||||
public NativeArray<int> colliderTypeCounts;
|
||||
|
||||
// auxiliar data:
|
||||
[ReadOnly] public BurstAffineTransform solverToWorld;
|
||||
[ReadOnly] public float deltaTime;
|
||||
[ReadOnly] public Oni.SolverParameters parameters;
|
||||
|
||||
public void Execute(int i)
|
||||
{
|
||||
int simplexStart = simplexCounts.GetSimplexStartAndSize(i, out int simplexSize);
|
||||
|
||||
// get all colliders overlapped by the cell bounds, in all grid levels:
|
||||
BurstAabb simplexBoundsWS = simplexBounds[i].Transformed(solverToWorld);
|
||||
NativeList<int> candidates = new NativeList<int>(16,Allocator.Temp);
|
||||
|
||||
// max size of the simplex bounds in cells:
|
||||
int3 maxSize = new int3(10);
|
||||
bool is2D = parameters.mode == Oni.SolverParameters.Mode.Mode2D;
|
||||
|
||||
for (int l = 0; l < gridLevels.Length; ++l)
|
||||
{
|
||||
float cellSize = NativeMultilevelGrid<int>.CellSizeOfLevel(gridLevels[l]);
|
||||
|
||||
int3 minCell = GridHash.Quantize(simplexBoundsWS.min.xyz, cellSize);
|
||||
int3 maxCell = GridHash.Quantize(simplexBoundsWS.max.xyz, cellSize);
|
||||
maxCell = minCell + math.min(maxCell - minCell, maxSize);
|
||||
|
||||
for (int x = minCell[0]; x <= maxCell[0]; ++x)
|
||||
{
|
||||
for (int y = minCell[1]; y <= maxCell[1]; ++y)
|
||||
{
|
||||
// for 2D mode, project each cell at z == 0 and check them too. This way we ensure 2D colliders
|
||||
// (which are inserted in cells with z == 0) are accounted for in the broadphase.
|
||||
if (is2D)
|
||||
{
|
||||
if (colliderGrid.TryGetCellIndex(new int4(x, y, 0, gridLevels[l]), out int cellIndex))
|
||||
{
|
||||
var colliderCell = colliderGrid.usedCells[cellIndex];
|
||||
candidates.AddRange(colliderCell.ContentsPointer, colliderCell.Length);
|
||||
}
|
||||
}
|
||||
|
||||
for (int z = minCell[2]; z <= maxCell[2]; ++z)
|
||||
{
|
||||
if (colliderGrid.TryGetCellIndex(new int4(x, y, z, gridLevels[l]), out int cellIndex))
|
||||
{
|
||||
var colliderCell = colliderGrid.usedCells[cellIndex];
|
||||
candidates.AddRange(colliderCell.ContentsPointer, colliderCell.Length);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (candidates.Length > 0)
|
||||
{
|
||||
// make sure each candidate collider only shows up once in the array:
|
||||
NativeArray<int> uniqueCandidates = candidates.AsArray();
|
||||
uniqueCandidates.Sort();
|
||||
int uniqueCount = uniqueCandidates.Unique();
|
||||
|
||||
// iterate over candidate colliders, generating contacts for each one
|
||||
for (int k = 0; k < uniqueCount; ++k)
|
||||
{
|
||||
int c = uniqueCandidates[k];
|
||||
if (c < shapes.Length)
|
||||
{
|
||||
BurstColliderShape shape = shapes[c];
|
||||
BurstAabb colliderBoundsWS = bounds[c];
|
||||
int rb = shape.rigidbodyIndex;
|
||||
|
||||
// Expand bounds by rigidbody's linear velocity:
|
||||
if (rb >= 0)
|
||||
colliderBoundsWS.Sweep(rigidbodies[rb].velocity * deltaTime);
|
||||
|
||||
// Expand bounds by collision material's stick distance:
|
||||
if (shape.materialIndex >= 0)
|
||||
colliderBoundsWS.Expand(collisionMaterials[shape.materialIndex].stickDistance);
|
||||
|
||||
// check if any simplex particle and the collider should collide:
|
||||
bool shouldCollide = false;
|
||||
var colliderCategory = shape.filter & ObiUtils.FilterCategoryBitmask;
|
||||
var colliderMask = (shape.filter & ObiUtils.FilterMaskBitmask) >> 16;
|
||||
for (int j = 0; j < simplexSize; ++j)
|
||||
{
|
||||
var simplexCategory = filters[simplices[simplexStart + j]] & ObiUtils.FilterCategoryBitmask;
|
||||
var simplexMask = (filters[simplices[simplexStart + j]] & ObiUtils.FilterMaskBitmask) >> 16;
|
||||
shouldCollide |= (simplexCategory & colliderMask) != 0 && (simplexMask & colliderCategory) != 0;
|
||||
}
|
||||
|
||||
if (shouldCollide && simplexBoundsWS.IntersectsAabb(in colliderBoundsWS, is2D))
|
||||
{
|
||||
// increment the amount of contacts for this shape type:
|
||||
Interlocked.Increment(ref ((int*)colliderTypeCounts.GetUnsafePtr())[(int)shape.type]);
|
||||
|
||||
// enqueue a new contact pair:
|
||||
contactPairQueue.Enqueue(new Oni.ContactPair{
|
||||
bodyA = i,
|
||||
bodyB = c
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
[BurstCompile]
|
||||
struct PrefixSumJob : IJob
|
||||
{
|
||||
[ReadOnly] public NativeArray<int> array;
|
||||
public NativeArray<int> sum;
|
||||
|
||||
public void Execute()
|
||||
{
|
||||
sum[0] = 0;
|
||||
for (int i = 1; i < sum.Length; ++i)
|
||||
sum[i] = sum[i - 1] + array[i-1];
|
||||
}
|
||||
}
|
||||
|
||||
[BurstCompile]
|
||||
struct SortContactPairsByShape : IJob
|
||||
{
|
||||
public NativeQueue<Oni.ContactPair> contactPairQueue;
|
||||
[ReadOnly] public NativeArray<BurstColliderShape> shapes;
|
||||
[ReadOnly] public NativeArray<int> start; // prefix sum
|
||||
public NativeArray<int> count;
|
||||
|
||||
public NativeList<Oni.ContactPair> contactPairs;
|
||||
|
||||
public void Execute()
|
||||
{
|
||||
contactPairs.ResizeUninitialized(contactPairQueue.Count);
|
||||
|
||||
while (!contactPairQueue.IsEmpty())
|
||||
{
|
||||
var pair = contactPairQueue.Dequeue();
|
||||
int shapeType = (int)shapes[pair.bodyB].type;
|
||||
|
||||
// write the pair directly at its position in the sorted array:
|
||||
contactPairs[start[shapeType] + (--count[shapeType])] = pair;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
[BurstCompile]
|
||||
unsafe struct ApplyForceZonesJob : IJobParallelFor
|
||||
{
|
||||
// particle arrays:
|
||||
[NativeDisableParallelForRestriction] public NativeArray<float4> externalForces;
|
||||
[NativeDisableParallelForRestriction] public NativeArray<float4> wind;
|
||||
[NativeDisableParallelForRestriction] public NativeArray<float4> velocities;
|
||||
[NativeDisableParallelForRestriction] public NativeArray<float4> colors;
|
||||
[NativeDisableParallelForRestriction] public NativeArray<float> life;
|
||||
[ReadOnly] public NativeArray<float4> positions;
|
||||
[ReadOnly] public NativeArray<float> invMasses;
|
||||
|
||||
// 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<ForceZone> forceZones;
|
||||
|
||||
// contacts
|
||||
[ReadOnly] public NativeArray<BurstContact> contacts;
|
||||
|
||||
// auxiliar data:
|
||||
[ReadOnly] public BurstAffineTransform worldToSolver;
|
||||
[ReadOnly] public float deltaTime;
|
||||
|
||||
public void Execute(int i)
|
||||
{
|
||||
var contact = contacts[i];
|
||||
int forceZoneIndex = shapes[contact.bodyB].forceZoneIndex;
|
||||
|
||||
if (forceZoneIndex >= 0)
|
||||
{
|
||||
int simplexStart = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSize);
|
||||
for (int j = 0; j < simplexSize; ++j)
|
||||
{
|
||||
int particleIndex = simplices[simplexStart + j];
|
||||
|
||||
float distance = -math.dot(positions[particleIndex] - contact.pointB, contact.normal);
|
||||
if (distance < 0) continue;
|
||||
|
||||
float4 axis = (worldToSolver * transforms[contact.bodyB]).TransformDirection(new float4(0, 0, 1, 0));
|
||||
|
||||
// calculate falloff region based on min/max distances:
|
||||
float falloff = 1;
|
||||
float range = forceZones[forceZoneIndex].maxDistance - forceZones[forceZoneIndex].minDistance;
|
||||
if (math.abs(range) > BurstMath.epsilon)
|
||||
falloff = math.pow(math.saturate((distance - forceZones[forceZoneIndex].minDistance) / range), forceZones[forceZoneIndex].falloffPower);
|
||||
|
||||
float forceIntensity = forceZones[forceZoneIndex].intensity * falloff;
|
||||
float dampIntensity = forceZones[forceZoneIndex].damping * falloff;
|
||||
|
||||
// tint particles:
|
||||
float mix = math.pow(1 - math.saturate(forceZones[forceZoneIndex].color.a * falloff), deltaTime);
|
||||
colors[particleIndex] = math.lerp((Vector4)forceZones[forceZoneIndex].color, colors[particleIndex], mix);
|
||||
|
||||
// calculate force direction, depending on the type of the force field:
|
||||
float4 result = float4.zero;
|
||||
switch (forceZones[forceZoneIndex].type)
|
||||
{
|
||||
case ForceZone.ZoneType.Radial:
|
||||
result = contact.normal * forceIntensity;
|
||||
break;
|
||||
case ForceZone.ZoneType.Vortex:
|
||||
result = new float4(math.cross(axis.xyz * forceIntensity, contact.normal.xyz).xyz, 0);
|
||||
break;
|
||||
case ForceZone.ZoneType.Directional:
|
||||
result = axis * forceIntensity;
|
||||
break;
|
||||
default:
|
||||
BurstMath.AtomicAdd(life, particleIndex, -forceIntensity * deltaTime);
|
||||
continue;
|
||||
}
|
||||
|
||||
// apply damping:
|
||||
switch (forceZones[forceZoneIndex].dampingDir)
|
||||
{
|
||||
case ForceZone.DampingDirection.ForceDirection:
|
||||
{
|
||||
float4 forceDir = math.normalizesafe(result);
|
||||
result -= forceDir * math.dot(velocities[particleIndex], forceDir) * dampIntensity;
|
||||
}
|
||||
break;
|
||||
case ForceZone.DampingDirection.SurfaceDirection:
|
||||
result -= contact.normal * math.dot(velocities[particleIndex], contact.normal) * dampIntensity;
|
||||
break;
|
||||
default:
|
||||
result -= velocities[particleIndex] * dampIntensity;
|
||||
break;
|
||||
}
|
||||
|
||||
if (invMasses[particleIndex] > 0)
|
||||
{
|
||||
switch (forceZones[forceZoneIndex].mode)
|
||||
{
|
||||
case ForceZone.ForceMode.Acceleration:
|
||||
BurstMath.AtomicAdd(externalForces, particleIndex, result / simplexSize / invMasses[particleIndex]);
|
||||
break;
|
||||
case ForceZone.ForceMode.Force:
|
||||
BurstMath.AtomicAdd(externalForces, particleIndex, result / simplexSize);
|
||||
break;
|
||||
case ForceZone.ForceMode.Wind:
|
||||
BurstMath.AtomicAdd(wind, particleIndex, result / simplexSize);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public JobHandle ApplyForceZones(BurstSolverImpl solver, float deltaTime, JobHandle inputDeps)
|
||||
{
|
||||
var world = ObiColliderWorld.GetInstance();
|
||||
|
||||
var applyForceFieldsJob = new ApplyForceZonesJob
|
||||
{
|
||||
contacts = solver.abstraction.colliderContacts.AsNativeArray<BurstContact>(),
|
||||
|
||||
positions = solver.positions,
|
||||
velocities = solver.velocities,
|
||||
externalForces = solver.externalForces,
|
||||
wind = solver.wind,
|
||||
invMasses = solver.invMasses,
|
||||
life = solver.life,
|
||||
colors = solver.colors,
|
||||
|
||||
simplices = solver.simplices,
|
||||
simplexCounts = solver.simplexCounts,
|
||||
|
||||
transforms = world.colliderTransforms.AsNativeArray<BurstAffineTransform>(),
|
||||
shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
|
||||
forceZones = world.forceZones.AsNativeArray<ForceZone>(),
|
||||
|
||||
worldToSolver = solver.worldToSolver,
|
||||
deltaTime = deltaTime,
|
||||
};
|
||||
|
||||
return applyForceFieldsJob.Schedule(solver.abstraction.colliderContacts.count, 64, inputDeps);
|
||||
}
|
||||
|
||||
public JobHandle GenerateContacts(BurstSolverImpl solver, float deltaTime, JobHandle inputDeps)
|
||||
{
|
||||
var world = ObiColliderWorld.GetInstance();
|
||||
|
||||
var generateColliderContactsJob = new GenerateContactsJob
|
||||
{
|
||||
colliderGrid = grid,
|
||||
gridLevels = grid.populatedLevels.GetKeyArray(Allocator.TempJob),
|
||||
|
||||
positions = solver.positions,
|
||||
orientations = solver.orientations,
|
||||
velocities = solver.velocities,
|
||||
invMasses = solver.invMasses,
|
||||
radii = solver.principalRadii,
|
||||
filters = solver.filters,
|
||||
particleMaterialIndices = solver.collisionMaterials,
|
||||
|
||||
simplices = solver.simplices,
|
||||
simplexCounts = solver.simplexCounts,
|
||||
simplexBounds = solver.simplexBounds,
|
||||
|
||||
transforms = world.colliderTransforms.AsNativeArray<BurstAffineTransform>(),
|
||||
shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
|
||||
rigidbodies = world.rigidbodies.AsNativeArray<BurstRigidbody>(),
|
||||
collisionMaterials = world.collisionMaterials.AsNativeArray<BurstCollisionMaterial>(),
|
||||
bounds = world.colliderAabbs.AsNativeArray<BurstAabb>(),
|
||||
|
||||
contactPairQueue = contactPairQueue.AsParallelWriter(),
|
||||
colliderTypeCounts = colliderTypeCounts,
|
||||
|
||||
solverToWorld = solver.solverToWorld,
|
||||
deltaTime = deltaTime,
|
||||
parameters = solver.abstraction.parameters
|
||||
};
|
||||
|
||||
inputDeps = generateColliderContactsJob.Schedule(solver.simplexCounts.simplexCount, 16, inputDeps);
|
||||
|
||||
var prefixSumJob = new PrefixSumJob
|
||||
{
|
||||
array = colliderTypeCounts,
|
||||
sum = contactOffsetsPerType
|
||||
};
|
||||
inputDeps = prefixSumJob.Schedule(inputDeps);
|
||||
|
||||
var sortPairsJob = new SortContactPairsByShape
|
||||
{
|
||||
contactPairQueue = contactPairQueue,
|
||||
shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
|
||||
start = contactOffsetsPerType,
|
||||
count = colliderTypeCounts,
|
||||
contactPairs = contactPairs
|
||||
};
|
||||
inputDeps = sortPairsJob.Schedule(inputDeps);
|
||||
|
||||
inputDeps.Complete();
|
||||
|
||||
inputDeps = BurstSphere.GenerateContacts(world,solver,contactPairs,colliderContactQueue,contactOffsetsPerType,deltaTime,inputDeps);
|
||||
inputDeps = BurstBox.GenerateContacts(world,solver,contactPairs,colliderContactQueue, contactOffsetsPerType,deltaTime,inputDeps);
|
||||
inputDeps = BurstCapsule.GenerateContacts(world, solver, contactPairs, colliderContactQueue, contactOffsetsPerType, deltaTime, inputDeps);
|
||||
inputDeps = BurstDistanceField.GenerateContacts(world, solver, contactPairs, colliderContactQueue, contactOffsetsPerType, deltaTime, inputDeps);
|
||||
inputDeps = BurstTriangleMesh.GenerateContacts(world, solver, contactPairs, colliderContactQueue, contactOffsetsPerType, deltaTime, inputDeps);
|
||||
inputDeps = BurstHeightField.GenerateContacts(world, solver, contactPairs, colliderContactQueue, contactOffsetsPerType, deltaTime, inputDeps);
|
||||
inputDeps = BurstEdgeMesh.GenerateContacts(world, solver, contactPairs, colliderContactQueue, contactOffsetsPerType, deltaTime, inputDeps);
|
||||
|
||||
return inputDeps;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
#endif
|
||||
@@ -1,11 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: f1a161c4294214a4fbcb7e9e94800494
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,65 +0,0 @@
|
||||
#if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
|
||||
using System;
|
||||
using System.Runtime.InteropServices;
|
||||
using UnityEngine;
|
||||
using Unity.Mathematics;
|
||||
|
||||
namespace Obi
|
||||
{
|
||||
public struct BurstDFNode
|
||||
{
|
||||
public float4 distancesA;
|
||||
public float4 distancesB;
|
||||
public float4 center;
|
||||
public int firstChild;
|
||||
|
||||
// add 12 bytes of padding to ensure correct memory alignment:
|
||||
private int pad0;
|
||||
private int pad1;
|
||||
private int pad2;
|
||||
|
||||
public float4 SampleWithGradient(float4 position)
|
||||
{
|
||||
float4 nPos = GetNormalizedPos(position);
|
||||
|
||||
// trilinear interpolation of distance:
|
||||
float4 x = distancesA + (distancesB - distancesA) * nPos[0];
|
||||
float2 y = x.xy + (x.zw - x.xy) * nPos[1];
|
||||
float distance = y[0] + (y[1] - y[0]) * nPos[2];
|
||||
|
||||
// gradient estimation:
|
||||
// x == 0
|
||||
float2 a = distancesA.xy + (distancesA.zw - distancesA.xy) * nPos[1];
|
||||
float x0 = a[0] + (a[1] - a[0]) * nPos[2];
|
||||
|
||||
// x == 1
|
||||
a = distancesB.xy + (distancesB.zw - distancesB.xy) * nPos[1];
|
||||
float x1 = a[0] + (a[1] - a[0]) * nPos[2];
|
||||
|
||||
// y == 0
|
||||
float y0 = x[0] + (x[1] - x[0]) * nPos[2];
|
||||
|
||||
// y == 1
|
||||
float y1 = x[2] + (x[3] - x[2]) * nPos[2];
|
||||
|
||||
return new float4(x1 - x0, y1 - y0, y[1] - y[0], distance);
|
||||
|
||||
}
|
||||
|
||||
public float4 GetNormalizedPos(float4 position)
|
||||
{
|
||||
float4 corner = center - new float4(center[3]);
|
||||
return (position - corner) / (center[3] * 2);
|
||||
}
|
||||
|
||||
public int GetOctant(float4 position)
|
||||
{
|
||||
int index = 0;
|
||||
if (position[0] > center[0]) index |= 4;
|
||||
if (position[1] > center[1]) index |= 2;
|
||||
if (position[2] > center[2]) index |= 1;
|
||||
return index;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
@@ -1,11 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: b5a1060edd11d4d268313295cdfe778b
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,188 +0,0 @@
|
||||
#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
|
||||
{
|
||||
public BurstColliderShape shape;
|
||||
public BurstAffineTransform colliderToSolver;
|
||||
|
||||
public NativeArray<DistanceFieldHeader> distanceFieldHeaders;
|
||||
public NativeArray<BurstDFNode> dfNodes;
|
||||
|
||||
public void Evaluate(float4 point, float4 radii, quaternion orientation, ref BurstLocalOptimization.SurfacePoint projectedPoint)
|
||||
{
|
||||
point = colliderToSolver.InverseTransformPoint(point);
|
||||
|
||||
if (shape.is2D)
|
||||
point[2] = 0;
|
||||
|
||||
var header = distanceFieldHeaders[shape.dataIndex];
|
||||
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);
|
||||
}
|
||||
|
||||
private float4 DFTraverse(float4 particlePosition, in DistanceFieldHeader header)
|
||||
{
|
||||
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
|
||||
};
|
||||
|
||||
|
||||
float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
|
||||
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;
|
||||
for (int j = 0; j < simplexSize; ++j)
|
||||
{
|
||||
int particleIndex = simplices[simplexStart + j];
|
||||
simplexRadius += radii[particleIndex].x * simplexBary[j];
|
||||
velocity += velocities[particleIndex] * simplexBary[j];
|
||||
}
|
||||
|
||||
float4 rbVelocity = float4.zero;
|
||||
if (rigidbodyIndex >= 0)
|
||||
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);
|
||||
|
||||
//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
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
||||
@@ -1,11 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 965f98194011e4cd9b8c1400b59565d8
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,211 +0,0 @@
|
||||
#if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
|
||||
using Unity.Burst;
|
||||
using Unity.Collections;
|
||||
using Unity.Jobs;
|
||||
using Unity.Mathematics;
|
||||
|
||||
namespace Obi
|
||||
{
|
||||
public struct BurstEdgeMesh : BurstLocalOptimization.IDistanceFunction
|
||||
{
|
||||
|
||||
public BurstColliderShape shape;
|
||||
public BurstAffineTransform colliderToSolver;
|
||||
public int dataOffset;
|
||||
|
||||
public EdgeMeshHeader header;
|
||||
public NativeArray<BIHNode> edgeBihNodes;
|
||||
public NativeArray<Edge> edges;
|
||||
public NativeArray<float2> vertices;
|
||||
|
||||
public void Evaluate(float4 point, float4 radii, quaternion orientation, ref BurstLocalOptimization.SurfacePoint projectedPoint)
|
||||
{
|
||||
point = colliderToSolver.InverseTransformPointUnscaled(point);
|
||||
|
||||
if (shape.is2D)
|
||||
point[2] = 0;
|
||||
|
||||
Edge t = edges[header.firstEdge + dataOffset];
|
||||
float4 v1 = (new float4(vertices[header.firstVertex + t.i1], 0, 0) + shape.center) * colliderToSolver.scale;
|
||||
float4 v2 = (new float4(vertices[header.firstVertex + t.i2], 0, 0) + shape.center) * colliderToSolver.scale;
|
||||
|
||||
float4 nearestPoint = BurstMath.NearestPointOnEdge(v1, v2, point, out float mu);
|
||||
float4 normal = math.normalizesafe(point - nearestPoint);
|
||||
|
||||
projectedPoint.normal = colliderToSolver.TransformDirection(normal);
|
||||
projectedPoint.point = colliderToSolver.TransformPointUnscaled(nearestPoint + normal * shape.contactOffset);
|
||||
}
|
||||
|
||||
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.EdgeMesh + 1] - contactOffsetsPerType[(int)Oni.ShapeType.EdgeMesh];
|
||||
if (pairCount == 0) return inputDeps;
|
||||
|
||||
var job = new GenerateEdgeMeshContactsJob
|
||||
{
|
||||
contactPairs = contactPairs,
|
||||
|
||||
positions = solver.positions,
|
||||
orientations = solver.orientations,
|
||||
velocities = solver.velocities,
|
||||
invMasses = solver.invMasses,
|
||||
radii = solver.principalRadii,
|
||||
|
||||
simplices = solver.simplices,
|
||||
simplexCounts = solver.simplexCounts,
|
||||
simplexBounds = solver.simplexBounds,
|
||||
|
||||
transforms = world.colliderTransforms.AsNativeArray<BurstAffineTransform>(),
|
||||
shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
|
||||
rigidbodies = world.rigidbodies.AsNativeArray<BurstRigidbody>(),
|
||||
|
||||
edgeMeshHeaders = world.edgeMeshContainer.headers.AsNativeArray<EdgeMeshHeader>(),
|
||||
edgeBihNodes = world.edgeMeshContainer.bihNodes.AsNativeArray<BIHNode>(),
|
||||
edges = world.edgeMeshContainer.edges.AsNativeArray<Edge>(),
|
||||
edgeVertices = world.edgeMeshContainer.vertices.AsNativeArray<float2>(),
|
||||
|
||||
contactsQueue = contactQueue.AsParallelWriter(),
|
||||
|
||||
solverToWorld = solver.solverToWorld,
|
||||
worldToSolver = solver.worldToSolver,
|
||||
deltaTime = deltaTime,
|
||||
parameters = solver.abstraction.parameters,
|
||||
firstPair = contactOffsetsPerType[(int)Oni.ShapeType.TriangleMesh]
|
||||
};
|
||||
|
||||
inputDeps = job.Schedule(pairCount, 1, inputDeps);
|
||||
return inputDeps;
|
||||
}
|
||||
}
|
||||
|
||||
[BurstCompile]
|
||||
struct GenerateEdgeMeshContactsJob : 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;
|
||||
[ReadOnly] public NativeArray<BurstAabb> simplexBounds;
|
||||
|
||||
// collider arrays:
|
||||
[ReadOnly] public NativeArray<BurstAffineTransform> transforms;
|
||||
[ReadOnly] public NativeArray<BurstColliderShape> shapes;
|
||||
[ReadOnly] public NativeArray<BurstRigidbody> rigidbodies;
|
||||
|
||||
// edge mesh data:
|
||||
[ReadOnly] public NativeArray<EdgeMeshHeader> edgeMeshHeaders;
|
||||
[ReadOnly] public NativeArray<BIHNode> edgeBihNodes;
|
||||
[ReadOnly] public NativeArray<Edge> edges;
|
||||
[ReadOnly] public NativeArray<float2> edgeVertices;
|
||||
|
||||
[WriteOnly]
|
||||
[NativeDisableParallelForRestriction]
|
||||
public NativeQueue<BurstContact>.ParallelWriter contactsQueue;
|
||||
|
||||
// auxiliar data:
|
||||
[ReadOnly] public int firstPair;
|
||||
[ReadOnly] public BurstAffineTransform 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;
|
||||
var shape = shapes[colliderIndex];
|
||||
|
||||
if (shape.dataIndex < 0)
|
||||
return;
|
||||
|
||||
var header = edgeMeshHeaders[shape.dataIndex];
|
||||
|
||||
int simplexStart = simplexCounts.GetSimplexStartAndSize(simplexIndex, out int simplexSize);
|
||||
var simplexBound = simplexBounds[simplexIndex];
|
||||
|
||||
BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
|
||||
|
||||
// invert a full matrix here to accurately represent collider bounds scale.
|
||||
var solverToCollider = math.inverse(float4x4.TRS(colliderToSolver.translation.xyz, colliderToSolver.rotation, colliderToSolver.scale.xyz));
|
||||
var simplexBoundsCS = simplexBound.Transformed(solverToCollider);
|
||||
|
||||
float4 marginCS = new float4((shape.contactOffset + parameters.collisionMargin) / colliderToSolver.scale.xyz, 0);
|
||||
|
||||
BurstEdgeMesh edgeMeshShape = new BurstEdgeMesh()
|
||||
{
|
||||
colliderToSolver = colliderToSolver,
|
||||
shape = shape,
|
||||
header = header,
|
||||
edgeBihNodes = edgeBihNodes,
|
||||
edges = edges,
|
||||
vertices = edgeVertices
|
||||
};
|
||||
|
||||
NativeQueue<int> queue = new NativeQueue<int>(Allocator.Temp);
|
||||
|
||||
queue.Enqueue(0);
|
||||
|
||||
while (!queue.IsEmpty())
|
||||
{
|
||||
int nodeIndex = queue.Dequeue();
|
||||
var node = edgeBihNodes[header.firstNode + nodeIndex];
|
||||
|
||||
// leaf node:
|
||||
if (node.firstChild < 0)
|
||||
{
|
||||
// check for contact against all triangles:
|
||||
for (int dataOffset = node.start; dataOffset < node.start + node.count; ++dataOffset)
|
||||
{
|
||||
Edge t = edges[header.firstEdge + dataOffset];
|
||||
float4 v1 = new float4(edgeVertices[header.firstVertex + t.i1], 0, 0) + shape.center;
|
||||
float4 v2 = new float4(edgeVertices[header.firstVertex + t.i2], 0, 0) + shape.center;
|
||||
BurstAabb edgeBounds = new BurstAabb(v1, v2, marginCS);
|
||||
|
||||
if (edgeBounds.IntersectsAabb(simplexBoundsCS, shape.is2D))
|
||||
{
|
||||
float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
|
||||
|
||||
edgeMeshShape.dataOffset = dataOffset;
|
||||
var colliderPoint = BurstLocalOptimization.Optimize(ref edgeMeshShape, positions, orientations, radii, simplices, simplexStart, simplexSize,
|
||||
ref simplexBary, out float4 convexPoint, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
|
||||
|
||||
contactsQueue.Enqueue(new BurstContact(){
|
||||
bodyA = simplexIndex,
|
||||
bodyB = colliderIndex,
|
||||
pointA = simplexBary,
|
||||
pointB = colliderPoint.point,
|
||||
normal = colliderPoint.normal * edgeMeshShape.shape.sign
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
else // check min and/or max children:
|
||||
{
|
||||
// visit min node:
|
||||
if (simplexBoundsCS.min[node.axis] <= node.min)
|
||||
queue.Enqueue(node.firstChild);
|
||||
|
||||
// visit max node:
|
||||
if (simplexBoundsCS.max[node.axis] >= node.max)
|
||||
queue.Enqueue(node.firstChild + 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
||||
@@ -1,11 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 91bd73e80795e494292d0b2fbf2e5e7f
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,273 +0,0 @@
|
||||
#if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
|
||||
using Unity.Burst;
|
||||
using Unity.Collections;
|
||||
using Unity.Jobs;
|
||||
using Unity.Mathematics;
|
||||
|
||||
namespace Obi
|
||||
{
|
||||
public struct BurstHeightField : BurstLocalOptimization.IDistanceFunction
|
||||
{
|
||||
public BurstColliderShape shape;
|
||||
public BurstAffineTransform colliderToSolver;
|
||||
|
||||
public BurstMath.CachedTri tri;
|
||||
public float4 triNormal;
|
||||
|
||||
public HeightFieldHeader header;
|
||||
public NativeArray<float> heightFieldSamples;
|
||||
|
||||
public void Evaluate(float4 point, float4 radii, quaternion orientation, ref BurstLocalOptimization.SurfacePoint projectedPoint)
|
||||
{
|
||||
point = colliderToSolver.InverseTransformPoint(point);
|
||||
|
||||
float4 nearestPoint = BurstMath.NearestPointOnTri(tri, point, out _);
|
||||
float4 normal = math.normalizesafe(point - nearestPoint);
|
||||
|
||||
// flip the contact normal if it points below ground: (doesn't work with holes)
|
||||
//BurstMath.OneSidedNormal(triNormal, ref normal);
|
||||
|
||||
projectedPoint.point = colliderToSolver.TransformPoint(nearestPoint + normal * shape.contactOffset);
|
||||
projectedPoint.normal = colliderToSolver.TransformDirection(normal);
|
||||
}
|
||||
|
||||
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.Heightmap + 1] - contactOffsetsPerType[(int)Oni.ShapeType.Heightmap];
|
||||
if (pairCount == 0) return inputDeps;
|
||||
|
||||
var job = new GenerateHeightFieldContactsJob
|
||||
{
|
||||
contactPairs = contactPairs,
|
||||
|
||||
positions = solver.positions,
|
||||
orientations = solver.orientations,
|
||||
velocities = solver.velocities,
|
||||
invMasses = solver.invMasses,
|
||||
radii = solver.principalRadii,
|
||||
|
||||
simplices = solver.simplices,
|
||||
simplexCounts = solver.simplexCounts,
|
||||
simplexBounds = solver.simplexBounds,
|
||||
|
||||
transforms = world.colliderTransforms.AsNativeArray<BurstAffineTransform>(),
|
||||
shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
|
||||
rigidbodies = world.rigidbodies.AsNativeArray<BurstRigidbody>(),
|
||||
|
||||
heightFieldHeaders = world.heightFieldContainer.headers.AsNativeArray<HeightFieldHeader>(),
|
||||
heightFieldSamples = world.heightFieldContainer.samples.AsNativeArray<float>(),
|
||||
|
||||
contactsQueue = contactQueue.AsParallelWriter(),
|
||||
|
||||
solverToWorld = solver.inertialFrame,
|
||||
worldToSolver = solver.worldToSolver,
|
||||
deltaTime = deltaTime,
|
||||
parameters = solver.abstraction.parameters,
|
||||
firstPair = contactOffsetsPerType[(int)Oni.ShapeType.Heightmap]
|
||||
};
|
||||
|
||||
inputDeps = job.Schedule(pairCount, 1, inputDeps);
|
||||
return inputDeps;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
[BurstCompile]
|
||||
struct GenerateHeightFieldContactsJob : 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;
|
||||
[ReadOnly] public NativeArray<BurstAabb> simplexBounds;
|
||||
|
||||
// collider arrays:
|
||||
[ReadOnly] public NativeArray<BurstAffineTransform> transforms;
|
||||
[ReadOnly] public NativeArray<BurstColliderShape> shapes;
|
||||
[ReadOnly] public NativeArray<BurstRigidbody> rigidbodies;
|
||||
|
||||
// height field data:
|
||||
[ReadOnly] public NativeArray<HeightFieldHeader> heightFieldHeaders;
|
||||
[ReadOnly] public NativeArray<float> heightFieldSamples;
|
||||
|
||||
[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;
|
||||
var shape = shapes[colliderIndex];
|
||||
|
||||
if (shape.dataIndex < 0)
|
||||
return;
|
||||
|
||||
var header = heightFieldHeaders[shape.dataIndex];
|
||||
int rigidbodyIndex = shapes[colliderIndex].rigidbodyIndex;
|
||||
|
||||
int simplexStart = simplexCounts.GetSimplexStartAndSize(simplexIndex, out int simplexSize);
|
||||
var simplexBound = simplexBounds[simplexIndex];
|
||||
|
||||
BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
|
||||
|
||||
// invert a full matrix here to accurately represent collider bounds scale.
|
||||
var solverToCollider = math.inverse(float4x4.TRS(colliderToSolver.translation.xyz, colliderToSolver.rotation, colliderToSolver.scale.xyz));
|
||||
var simplexBoundsCS = simplexBound.Transformed(solverToCollider);
|
||||
|
||||
BurstHeightField triangleMeshShape = new BurstHeightField()
|
||||
{
|
||||
colliderToSolver = colliderToSolver,
|
||||
shape = shapes[colliderIndex],
|
||||
header = heightFieldHeaders[shapes[colliderIndex].dataIndex],
|
||||
heightFieldSamples = heightFieldSamples
|
||||
};
|
||||
|
||||
float4 triNormal = float4.zero;
|
||||
|
||||
var co = new BurstContact { bodyA = simplexIndex, bodyB = 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(simplexBoundsCS.min[0] / cellWidth), (int)math.floor(simplexBoundsCS.min[2] / cellHeight));
|
||||
int2 max = new int2((int)math.floor(simplexBoundsCS.max[0] / cellWidth), (int)math.floor(simplexBoundsCS.max[2] / 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 = heightFieldSamples[header.firstSample + sv * resolutionU + su] * shape.size.y;
|
||||
float h2 = heightFieldSamples[header.firstSample + sv * resolutionU + csu1] * shape.size.y;
|
||||
float h3 = heightFieldSamples[header.firstSample + csv1 * resolutionU + su] * shape.size.y;
|
||||
float h4 = heightFieldSamples[header.firstSample + csv1 * resolutionU + csu1] * shape.size.y;
|
||||
|
||||
if (h1 < 0) continue;
|
||||
h1 = math.abs(h1);
|
||||
h2 = math.abs(h2);
|
||||
h3 = math.abs(h3);
|
||||
h4 = math.abs(h4);
|
||||
|
||||
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);
|
||||
|
||||
float4 convexPoint;
|
||||
float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
|
||||
|
||||
// ------contact against the first triangle------:
|
||||
float4 v1 = new float4(min_x, h3, max_z, 0);
|
||||
float4 v2 = new float4(max_x, h4, max_z, 0);
|
||||
float4 v3 = new float4(min_x, h1, min_z, 0);
|
||||
|
||||
triangleMeshShape.tri.Cache(v1, v2, v3);
|
||||
triNormal.xyz = math.normalizesafe(math.cross((v2 - v1).xyz, (v3 - v1).xyz));
|
||||
|
||||
var colliderPoint = BurstLocalOptimization.Optimize(ref triangleMeshShape, positions, orientations, radii, simplices, simplexStart, simplexSize,
|
||||
ref simplexBary, out convexPoint, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
|
||||
|
||||
float4 velocity = float4.zero;
|
||||
float simplexRadius = 0;
|
||||
for (int j = 0; j < simplexSize; ++j)
|
||||
{
|
||||
int particleIndex = simplices[simplexStart + j];
|
||||
simplexRadius += radii[particleIndex].x * simplexBary[j];
|
||||
velocity += velocities[particleIndex] * simplexBary[j];
|
||||
}
|
||||
|
||||
float4 rbVelocity = float4.zero;
|
||||
if (rigidbodyIndex >= 0)
|
||||
rbVelocity = BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, colliderPoint.point, rigidbodies, solverToWorld);
|
||||
|
||||
float dAB = math.dot(convexPoint - colliderPoint.point, colliderPoint.normal);
|
||||
float vel = math.dot(velocity - rbVelocity, colliderPoint.normal);
|
||||
|
||||
if (vel * deltaTime + dAB <= simplexRadius + shape.contactOffset + parameters.collisionMargin)
|
||||
{
|
||||
co.pointB = colliderPoint.point;
|
||||
co.normal = colliderPoint.normal * triangleMeshShape.shape.sign;
|
||||
co.pointA = simplexBary;
|
||||
contactsQueue.Enqueue(co);
|
||||
}
|
||||
|
||||
// ------contact against the second triangle------:
|
||||
v1 = new float4(min_x, h1, min_z, 0);
|
||||
v2 = new float4(max_x, h4, max_z, 0);
|
||||
v3 = new float4(max_x, h2, min_z, 0);
|
||||
|
||||
triangleMeshShape.tri.Cache(v1, v2, v3);
|
||||
triNormal.xyz = math.normalizesafe(math.cross((v2 - v1).xyz, (v3 - v1).xyz));
|
||||
|
||||
colliderPoint = BurstLocalOptimization.Optimize(ref triangleMeshShape, positions, orientations, radii, simplices, simplexStart, simplexSize,
|
||||
ref simplexBary, out convexPoint, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
|
||||
|
||||
velocity = float4.zero;
|
||||
simplexRadius = 0;
|
||||
for (int j = 0; j < simplexSize; ++j)
|
||||
{
|
||||
int particleIndex = simplices[simplexStart + j];
|
||||
simplexRadius += radii[particleIndex].x * simplexBary[j];
|
||||
velocity += velocities[particleIndex] * simplexBary[j];
|
||||
}
|
||||
|
||||
rbVelocity = float4.zero;
|
||||
if (rigidbodyIndex >= 0)
|
||||
rbVelocity = BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, colliderPoint.point, rigidbodies, solverToWorld);
|
||||
|
||||
dAB = math.dot(convexPoint - colliderPoint.point, colliderPoint.normal);
|
||||
vel = math.dot(velocity - rbVelocity, colliderPoint.normal);
|
||||
|
||||
if (vel * deltaTime + dAB <= simplexRadius + shape.contactOffset + parameters.collisionMargin)
|
||||
{
|
||||
co.pointB = colliderPoint.point;
|
||||
co.normal = colliderPoint.normal * triangleMeshShape.shape.sign;
|
||||
co.pointA = simplexBary;
|
||||
|
||||
contactsQueue.Enqueue(co);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
||||
@@ -1,11 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 780062b6393e3476e947fb10fc9b4f3d
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,207 +0,0 @@
|
||||
#if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using UnityEngine;
|
||||
using Unity.Collections;
|
||||
using Unity.Jobs;
|
||||
using Unity.Mathematics;
|
||||
using Unity.Burst;
|
||||
using System.Runtime.CompilerServices;
|
||||
|
||||
namespace Obi
|
||||
{
|
||||
public static class BurstLocalOptimization
|
||||
{
|
||||
|
||||
/**
|
||||
* point in the surface of a signed distance field.
|
||||
*/
|
||||
public struct SurfacePoint
|
||||
{
|
||||
public float4 bary;
|
||||
public float4 point;
|
||||
public float4 normal;
|
||||
}
|
||||
|
||||
public interface IDistanceFunction
|
||||
{
|
||||
void Evaluate(float4 point, float4 radii, quaternion orientation, ref SurfacePoint projectedPoint);
|
||||
}
|
||||
|
||||
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
||||
private static void GetInterpolatedSimplexData(int simplexStart,
|
||||
int simplexSize,
|
||||
NativeArray<int> simplices,
|
||||
NativeArray<float4> positions,
|
||||
NativeArray<quaternion> orientations,
|
||||
NativeArray<float4> radii,
|
||||
float4 convexBary,
|
||||
out float4 convexPoint,
|
||||
out float4 convexRadii,
|
||||
out quaternion convexOrientation)
|
||||
{
|
||||
convexPoint = float4.zero;
|
||||
convexRadii = float4.zero;
|
||||
convexOrientation = new quaternion(0, 0, 0, 0);
|
||||
for (int j = 0; j < simplexSize; ++j)
|
||||
{
|
||||
int particle = simplices[simplexStart + j];
|
||||
convexPoint += positions[particle] * convexBary[j];
|
||||
convexRadii += radii[particle] * convexBary[j];
|
||||
convexOrientation.value += orientations[particle].value * convexBary[j];
|
||||
}
|
||||
convexPoint.w = 0;
|
||||
}
|
||||
|
||||
public static SurfacePoint Optimize<T>(ref T function,
|
||||
NativeArray<float4> positions,
|
||||
NativeArray<quaternion> orientations,
|
||||
NativeArray<float4> radii,
|
||||
NativeArray<int> simplices,
|
||||
int simplexStart,
|
||||
int simplexSize,
|
||||
ref float4 convexBary,
|
||||
out float4 convexPoint,
|
||||
int maxIterations = 16,
|
||||
float tolerance = 0.004f) where T : struct, IDistanceFunction
|
||||
{
|
||||
var pointInFunction = new SurfacePoint();
|
||||
|
||||
// get cartesian coordinates of the initial guess:
|
||||
GetInterpolatedSimplexData(simplexStart, simplexSize, simplices, positions, orientations, radii, convexBary, out convexPoint, out float4 convexThickness, out quaternion convexOrientation);
|
||||
|
||||
// for a 0-simplex (point), perform a single evaluation:
|
||||
if (simplexSize == 1 || maxIterations < 1)
|
||||
function.Evaluate(convexPoint, convexThickness, convexOrientation, ref pointInFunction);
|
||||
|
||||
// for a 1-simplex (edge), perform golden ratio search:
|
||||
else if (simplexSize == 2)
|
||||
GoldenSearch(ref function, simplexStart, simplexSize, positions, orientations, radii, simplices, ref convexPoint, ref convexThickness, ref convexOrientation, ref convexBary, ref pointInFunction, maxIterations, tolerance * 10);
|
||||
|
||||
// for higher-order simplices, use general Frank-Wolfe convex optimization:
|
||||
else
|
||||
FrankWolfe(ref function, simplexStart, simplexSize, positions, orientations, radii, simplices, ref convexPoint, ref convexThickness, ref convexOrientation, ref convexBary, ref pointInFunction, maxIterations, tolerance);
|
||||
|
||||
return pointInFunction;
|
||||
}
|
||||
|
||||
// Frank-Wolfe convex optimization algorithm. Returns closest point to a simplex in a signed distance function.
|
||||
private static void FrankWolfe<T>(ref T function,
|
||||
int simplexStart,
|
||||
int simplexSize,
|
||||
NativeArray<float4> positions,
|
||||
NativeArray<quaternion> orientations,
|
||||
NativeArray<float4> radii,
|
||||
NativeArray<int> simplices,
|
||||
ref float4 convexPoint,
|
||||
ref float4 convexThickness,
|
||||
ref quaternion convexOrientation,
|
||||
ref float4 convexBary,
|
||||
ref SurfacePoint pointInFunction,
|
||||
int maxIterations,
|
||||
float tolerance) where T : struct, IDistanceFunction
|
||||
{
|
||||
for (int i = 0; i < maxIterations; ++i)
|
||||
{
|
||||
// sample target function:
|
||||
function.Evaluate(convexPoint, convexThickness, convexOrientation, ref pointInFunction);
|
||||
|
||||
// find descent direction:
|
||||
int descent = 0;
|
||||
float gap = float.MinValue;
|
||||
for (int j = 0; j < simplexSize; ++j)
|
||||
{
|
||||
int particle = simplices[simplexStart + j];
|
||||
float4 candidate = positions[particle] - convexPoint;
|
||||
candidate.w = 0;
|
||||
|
||||
// here, we adjust the candidate by projecting it to the engrosed simplex's surface:
|
||||
candidate -= pointInFunction.normal * (radii[particle].x - convexThickness.x);
|
||||
|
||||
float corr = math.dot(-pointInFunction.normal, candidate);
|
||||
if (corr > gap)
|
||||
{
|
||||
descent = j;
|
||||
gap = corr;
|
||||
}
|
||||
}
|
||||
|
||||
// if the duality gap is below tolerance threshold, stop iterating.
|
||||
if (gap < tolerance)
|
||||
break;
|
||||
|
||||
// update the barycentric coords using 2/(i+2) as the step factor
|
||||
float step = 0.3f * 2.0f / (i + 2);
|
||||
convexBary *= 1 - step;
|
||||
convexBary[descent] += step;
|
||||
|
||||
// get cartesian coordinates of current solution:
|
||||
GetInterpolatedSimplexData(simplexStart, simplexSize, simplices, positions, orientations, radii, convexBary, out convexPoint, out convexThickness, out convexOrientation);
|
||||
}
|
||||
}
|
||||
|
||||
private static void GoldenSearch<T>(ref T function,
|
||||
int simplexStart,
|
||||
int simplexSize,
|
||||
NativeArray<float4> positions,
|
||||
NativeArray<quaternion> orientations,
|
||||
NativeArray<float4> radii,
|
||||
NativeArray<int> simplices,
|
||||
ref float4 convexPoint,
|
||||
ref float4 convexThickness,
|
||||
ref quaternion convexOrientation,
|
||||
ref float4 convexBary,
|
||||
ref SurfacePoint pointInFunction,
|
||||
int maxIterations,
|
||||
float tolerance) where T : struct, IDistanceFunction
|
||||
{
|
||||
var pointInFunctionD = new SurfacePoint();
|
||||
float4 convexPointD, convexThicknessD;
|
||||
quaternion convexOrientationD;
|
||||
|
||||
float gr = (math.sqrt(5.0f) + 1) / 2.0f;
|
||||
float u = 0, v = 1;
|
||||
float c = v - (v - u) / gr;
|
||||
float d = u + (v - u) / gr;
|
||||
|
||||
for (int i = 0; i < maxIterations; ++i)
|
||||
{
|
||||
// if the gap is below tolerance threshold, stop iterating.
|
||||
if (math.abs(v - u) < tolerance * (math.abs(c) + math.abs(d)))
|
||||
break;
|
||||
|
||||
GetInterpolatedSimplexData(simplexStart, simplexSize, simplices, positions, orientations, radii, new float4(c, 1 - c, 0, 0), out convexPoint, out convexThickness, out convexOrientation);
|
||||
GetInterpolatedSimplexData(simplexStart, simplexSize, simplices, positions, orientations, radii, new float4(d, 1 - d, 0, 0), out convexPointD, out convexThicknessD, out convexOrientationD);
|
||||
|
||||
function.Evaluate(convexPoint, convexThickness, convexOrientation, ref pointInFunction);
|
||||
function.Evaluate(convexPointD, convexThicknessD, convexOrientationD, ref pointInFunctionD);
|
||||
|
||||
float4 candidateC = positions[simplices[simplexStart]] - pointInFunction.point;
|
||||
float4 candidateD = positions[simplices[simplexStart + 1]] - pointInFunctionD.point;
|
||||
candidateC.w = 0;
|
||||
candidateD.w = 0;
|
||||
|
||||
candidateC -= pointInFunction.normal * (radii[simplices[simplexStart]].x - convexThickness.x);
|
||||
candidateD -= pointInFunctionD.normal * (radii[simplices[simplexStart + 1]].x - convexThicknessD.x);
|
||||
|
||||
if (math.dot(-pointInFunction.normal, candidateC) < math.dot(-pointInFunctionD.normal, candidateD))
|
||||
v = d;
|
||||
else
|
||||
u = c;
|
||||
|
||||
c = v - (v - u) / gr;
|
||||
d = u + (v - u) / gr;
|
||||
}
|
||||
|
||||
float mid = (v + u) * 0.5f;
|
||||
convexBary.x = mid;
|
||||
convexBary.y = (1 - mid);
|
||||
GetInterpolatedSimplexData(simplexStart, simplexSize, simplices, positions, orientations, radii, convexBary, out convexPoint, out convexThickness, out convexOrientation);
|
||||
function.Evaluate(convexPoint, convexThickness, convexOrientation, ref pointInFunction);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
||||
@@ -1,11 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: e0353c4eae8ca4ec78f2db34845cd5cd
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,65 +0,0 @@
|
||||
#if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using UnityEngine;
|
||||
using Unity.Collections;
|
||||
using Unity.Jobs;
|
||||
using Unity.Mathematics;
|
||||
using Unity.Burst;
|
||||
|
||||
namespace Obi
|
||||
{
|
||||
public struct BurstSimplex : BurstLocalOptimization.IDistanceFunction
|
||||
{
|
||||
public NativeArray<float4> positions;
|
||||
public NativeArray<float4> radii;
|
||||
public NativeArray<int> simplices;
|
||||
|
||||
public int simplexStart;
|
||||
public int simplexSize;
|
||||
|
||||
private BurstMath.CachedTri tri;
|
||||
|
||||
public void CacheData()
|
||||
{
|
||||
if (simplexSize == 3)
|
||||
{
|
||||
tri.Cache(new float4(positions[simplices[simplexStart]].xyz,0),
|
||||
new float4(positions[simplices[simplexStart + 1]].xyz,0),
|
||||
new float4(positions[simplices[simplexStart + 2]].xyz,0));
|
||||
}
|
||||
}
|
||||
|
||||
public void Evaluate(float4 point, float4 radii, quaternion orientation, ref BurstLocalOptimization.SurfacePoint projectedPoint)
|
||||
{
|
||||
switch (simplexSize)
|
||||
{
|
||||
case 1:
|
||||
default:
|
||||
{
|
||||
float4 p1 = positions[simplices[simplexStart]]; p1.w = 0;
|
||||
projectedPoint.bary = new float4(1, 0, 0, 0);
|
||||
projectedPoint.point = p1;
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
{
|
||||
float4 p1 = positions[simplices[simplexStart]]; p1.w = 0;
|
||||
float4 p2 = positions[simplices[simplexStart + 1]]; p2.w = 0;
|
||||
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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
||||
@@ -1,11 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: d84f433832a8c4a90ba92ac471d18e44
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,127 +0,0 @@
|
||||
#if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
|
||||
using Unity.Burst;
|
||||
using Unity.Collections;
|
||||
using Unity.Jobs;
|
||||
using Unity.Mathematics;
|
||||
|
||||
namespace Obi
|
||||
{
|
||||
public struct BurstSphere : BurstLocalOptimization.IDistanceFunction
|
||||
{
|
||||
public BurstColliderShape shape;
|
||||
public BurstAffineTransform colliderToSolver;
|
||||
|
||||
public void Evaluate(float4 point, float4 radii, quaternion orientation, ref BurstLocalOptimization.SurfacePoint projectedPoint)
|
||||
{
|
||||
float4 center = shape.center * colliderToSolver.scale;
|
||||
point = colliderToSolver.InverseTransformPointUnscaled(point) - center;
|
||||
|
||||
if (shape.is2D)
|
||||
point[2] = 0;
|
||||
|
||||
float radius = shape.size.x * math.cmax(colliderToSolver.scale.xyz);
|
||||
float distanceToCenter = math.length(point);
|
||||
|
||||
float4 normal = point / (distanceToCenter + BurstMath.epsilon);
|
||||
|
||||
projectedPoint.point = colliderToSolver.TransformPointUnscaled(center + normal * (radius + shape.contactOffset));
|
||||
projectedPoint.normal = colliderToSolver.TransformDirection(normal);
|
||||
}
|
||||
|
||||
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.Sphere + 1] - contactOffsetsPerType[(int)Oni.ShapeType.Sphere];
|
||||
if (pairCount == 0) return inputDeps;
|
||||
|
||||
var job = new GenerateSphereContactsJob
|
||||
{
|
||||
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>(),
|
||||
|
||||
contactsQueue = contactQueue.AsParallelWriter(),
|
||||
|
||||
worldToSolver = solver.worldToSolver,
|
||||
deltaTime = deltaTime,
|
||||
parameters = solver.abstraction.parameters,
|
||||
firstPair = contactOffsetsPerType[(int)Oni.ShapeType.Sphere]
|
||||
};
|
||||
|
||||
inputDeps = job.Schedule(pairCount, 8, inputDeps);
|
||||
return inputDeps;
|
||||
}
|
||||
}
|
||||
|
||||
[BurstCompile]
|
||||
struct GenerateSphereContactsJob : 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;
|
||||
|
||||
[WriteOnly]
|
||||
[NativeDisableParallelForRestriction]
|
||||
public NativeQueue<BurstContact>.ParallelWriter contactsQueue;
|
||||
|
||||
// auxiliar data:
|
||||
[ReadOnly] public int firstPair;
|
||||
[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 simplexStart = simplexCounts.GetSimplexStartAndSize(simplexIndex, out int simplexSize);
|
||||
|
||||
BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
|
||||
|
||||
BurstSphere shape = new BurstSphere { colliderToSolver = colliderToSolver, shape = shapes[colliderIndex] };
|
||||
|
||||
float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
|
||||
var colliderPoint = BurstLocalOptimization.Optimize(ref shape, positions, orientations, radii, simplices, simplexStart, simplexSize,
|
||||
ref simplexBary, out _, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
|
||||
|
||||
contactsQueue.Enqueue(new BurstContact {
|
||||
bodyA = simplexIndex,
|
||||
bodyB = colliderIndex,
|
||||
pointA = simplexBary,
|
||||
pointB = colliderPoint.point,
|
||||
normal = colliderPoint.normal * shape.shape.sign
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
||||
@@ -1,11 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 38081dcc3e28a4a3dba1e8e5d042a262
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,220 +0,0 @@
|
||||
#if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
|
||||
using Unity.Burst;
|
||||
using Unity.Collections;
|
||||
using Unity.Jobs;
|
||||
using Unity.Mathematics;
|
||||
|
||||
namespace Obi
|
||||
{
|
||||
public struct BurstTriangleMesh : BurstLocalOptimization.IDistanceFunction
|
||||
{
|
||||
public BurstColliderShape shape;
|
||||
public BurstAffineTransform colliderToSolver;
|
||||
|
||||
public BurstMath.CachedTri tri;
|
||||
|
||||
public void Evaluate(float4 point, float4 radii, quaternion orientation, ref BurstLocalOptimization.SurfacePoint projectedPoint)
|
||||
{
|
||||
point = colliderToSolver.InverseTransformPointUnscaled(point);
|
||||
|
||||
if (shape.is2D)
|
||||
point[2] = 0;
|
||||
|
||||
float4 nearestPoint = BurstMath.NearestPointOnTri(tri, point, out float4 bary);
|
||||
float4 normal = math.normalizesafe(point - nearestPoint);
|
||||
|
||||
projectedPoint.point = colliderToSolver.TransformPointUnscaled(nearestPoint + normal * shape.contactOffset);
|
||||
projectedPoint.normal = colliderToSolver.TransformDirection(normal);
|
||||
}
|
||||
|
||||
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.TriangleMesh + 1] - contactOffsetsPerType[(int)Oni.ShapeType.TriangleMesh];
|
||||
if (pairCount == 0) return inputDeps;
|
||||
|
||||
var job = new GenerateTriangleMeshContactsJob
|
||||
{
|
||||
contactPairs = contactPairs,
|
||||
|
||||
positions = solver.positions,
|
||||
orientations = solver.orientations,
|
||||
velocities = solver.velocities,
|
||||
invMasses = solver.invMasses,
|
||||
radii = solver.principalRadii,
|
||||
|
||||
simplices = solver.simplices,
|
||||
simplexCounts = solver.simplexCounts,
|
||||
simplexBounds = solver.simplexBounds,
|
||||
|
||||
transforms = world.colliderTransforms.AsNativeArray<BurstAffineTransform>(),
|
||||
shapes = world.colliderShapes.AsNativeArray<BurstColliderShape>(),
|
||||
rigidbodies = world.rigidbodies.AsNativeArray<BurstRigidbody>(),
|
||||
|
||||
triangleMeshHeaders = world.triangleMeshContainer.headers.AsNativeArray<TriangleMeshHeader>(),
|
||||
bihNodes = world.triangleMeshContainer.bihNodes.AsNativeArray<BIHNode>(),
|
||||
triangles = world.triangleMeshContainer.triangles.AsNativeArray<Triangle>(),
|
||||
vertices = world.triangleMeshContainer.vertices.AsNativeArray<float3>(),
|
||||
|
||||
contactsQueue = contactQueue.AsParallelWriter(),
|
||||
|
||||
solverToWorld = solver.inertialFrame,
|
||||
worldToSolver = solver.worldToSolver,
|
||||
deltaTime = deltaTime,
|
||||
parameters = solver.abstraction.parameters,
|
||||
firstPair = contactOffsetsPerType[(int)Oni.ShapeType.TriangleMesh]
|
||||
};
|
||||
|
||||
inputDeps = job.Schedule(pairCount, 1, inputDeps);
|
||||
return inputDeps;
|
||||
}
|
||||
}
|
||||
|
||||
[BurstCompile]
|
||||
struct GenerateTriangleMeshContactsJob : 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;
|
||||
[ReadOnly] public NativeArray<BurstAabb> simplexBounds;
|
||||
|
||||
// collider arrays:
|
||||
[ReadOnly] public NativeArray<BurstAffineTransform> transforms;
|
||||
[ReadOnly] public NativeArray<BurstColliderShape> shapes;
|
||||
[ReadOnly] public NativeArray<BurstRigidbody> rigidbodies;
|
||||
|
||||
// triangle mesh data:
|
||||
[ReadOnly] public NativeArray<TriangleMeshHeader> triangleMeshHeaders;
|
||||
[ReadOnly] public NativeArray<BIHNode> bihNodes;
|
||||
[ReadOnly] public NativeArray<Triangle> triangles;
|
||||
[ReadOnly] public NativeArray<float3> vertices;
|
||||
|
||||
[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;
|
||||
var shape = shapes[colliderIndex];
|
||||
|
||||
if (shape.dataIndex < 0)
|
||||
return;
|
||||
|
||||
int rigidbodyIndex = shape.rigidbodyIndex;
|
||||
var header = triangleMeshHeaders[shape.dataIndex];
|
||||
|
||||
int simplexStart = simplexCounts.GetSimplexStartAndSize(simplexIndex, out int simplexSize);
|
||||
var simplexBound = simplexBounds[simplexIndex];
|
||||
|
||||
BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
|
||||
|
||||
// invert a full matrix here to accurately represent collider bounds scale.
|
||||
var solverToCollider = math.inverse(float4x4.TRS(colliderToSolver.translation.xyz, colliderToSolver.rotation, colliderToSolver.scale.xyz));
|
||||
var simplexBoundsCS = simplexBound.Transformed(solverToCollider);
|
||||
|
||||
float4 marginCS = new float4((shape.contactOffset + parameters.collisionMargin) / colliderToSolver.scale.xyz, 0);
|
||||
|
||||
BurstTriangleMesh triangleMeshShape = new BurstTriangleMesh()
|
||||
{
|
||||
colliderToSolver = colliderToSolver,
|
||||
shape = shape
|
||||
};
|
||||
|
||||
NativeQueue<int> queue = new NativeQueue<int>(Allocator.Temp);
|
||||
|
||||
queue.Enqueue(0);
|
||||
|
||||
while (!queue.IsEmpty())
|
||||
{
|
||||
int nodeIndex = queue.Dequeue();
|
||||
var node = bihNodes[header.firstNode + nodeIndex];
|
||||
|
||||
// leaf node:
|
||||
if (node.firstChild < 0)
|
||||
{
|
||||
// check for contact against all triangles:
|
||||
for (int dataOffset = node.start; dataOffset < node.start + node.count; ++dataOffset)
|
||||
{
|
||||
Triangle t = triangles[header.firstTriangle + dataOffset];
|
||||
float4 v1 = new float4(vertices[header.firstVertex + t.i1], 0);
|
||||
float4 v2 = new float4(vertices[header.firstVertex + t.i2], 0);
|
||||
float4 v3 = new float4(vertices[header.firstVertex + t.i3], 0);
|
||||
BurstAabb triangleBounds = new BurstAabb(v1, v2, v3, marginCS);
|
||||
|
||||
if (triangleBounds.IntersectsAabb(simplexBoundsCS, shape.is2D))
|
||||
{
|
||||
float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);
|
||||
|
||||
triangleMeshShape.tri.Cache(v1 * colliderToSolver.scale, v2 * colliderToSolver.scale, v3 * colliderToSolver.scale);
|
||||
|
||||
var colliderPoint = BurstLocalOptimization.Optimize(ref triangleMeshShape, positions, orientations, radii, simplices, simplexStart, simplexSize,
|
||||
ref simplexBary, out float4 simplexPoint, parameters.surfaceCollisionIterations, parameters.surfaceCollisionTolerance);
|
||||
|
||||
float4 velocity = float4.zero;
|
||||
float simplexRadius = 0;
|
||||
for (int j = 0; j < simplexSize; ++j)
|
||||
{
|
||||
int particleIndex = simplices[simplexStart + j];
|
||||
simplexRadius += radii[particleIndex].x * simplexBary[j];
|
||||
velocity += velocities[particleIndex] * simplexBary[j];
|
||||
}
|
||||
|
||||
float4 rbVelocity = float4.zero;
|
||||
if (rigidbodyIndex >= 0)
|
||||
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);
|
||||
|
||||
if (vel * deltaTime + dAB <= simplexRadius + shape.contactOffset + parameters.collisionMargin)
|
||||
{
|
||||
contactsQueue.Enqueue(new BurstContact()
|
||||
{
|
||||
bodyA = simplexIndex,
|
||||
bodyB = colliderIndex,
|
||||
pointA = simplexBary,
|
||||
pointB = colliderPoint.point,
|
||||
normal = colliderPoint.normal * triangleMeshShape.shape.sign
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else // check min and/or max children:
|
||||
{
|
||||
// visit min node:
|
||||
if (simplexBoundsCS.min[node.axis] <= node.min)
|
||||
queue.Enqueue(node.firstChild);
|
||||
|
||||
// visit max node:
|
||||
if (simplexBoundsCS.max[node.axis] >= node.max)
|
||||
queue.Enqueue(node.firstChild + 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
@@ -1,11 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 1a65b8b437c2548c0b04b7ddb58a3970
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
Reference in New Issue
Block a user