public static RigidbodyHandle GetNewRigidbody()
{
Span <RigidbodyState> span = editState.Span;
for (int i = 0; i < span.Length; i++)
{
if (!span[i].IsAssigned)
{
span[i] = new RigidbodyState();
span[i].IsAssigned = true;
span[i].transform = new Transform(Vector3.Zero);
span[i].mass = 1;
span[i].inertia = Vector3.One;
RigidbodyHandle handle = new RigidbodyHandle((uint)i);
handles.Add(handle);
return(handle);
}
}
editState.Grow();
oldState.Grow();
interpolatedState.Grow();
newState.Grow();
return(GetNewRigidbody());
}
private static List <ContactPoint> NarrowPhase(Memory <CollidablePair> pairs)
{
List <ContactPoint> contacts = new List <ContactPoint>();
Span <CollidablePair> pa = pairs.Span;
for (int i = 0; i < pa.Length; i++)
{
RigidbodyHandle a = (RigidbodyHandle)pa[i].a;
RigidbodyHandle b = (RigidbodyHandle)pa[i].b;
if (a.GetBound().IsColliding(a.state[0].transform, b.state[0].transform, b.GetBound()).Length > 0)
{
contacts.AddRange(a.shape.IsColliding(a.state[0].transform, b.state[0].transform, b.shape));
}
}
return(contacts);
}
public static List <CollidablePair> SweepAndSplit(List <AABB> aABBs)
{
List <AABBMarker> markersx = new List <AABBMarker>(aABBs.Count * 2);
for (int i = 0; i < aABBs.Count; i++)
{
AABB b = aABBs[i];
markersx.Add(new MinAABB {
a = aABBs[i], position = b.WorldMin(handles[i].state[0].transform.position)
});
markersx.Add(new MaxAABB {
a = aABBs[i], position = b.WorldMax(handles[i].state[0].transform.position)
});
}
List <List <AABBMarker> > start = new List <List <AABBMarker> >();
start.Add(markersx);
List <List <AABBMarker> > final = Split(Split(Split(start, Vector3.UnitX), Vector3.UnitY), Vector3.UnitZ);
List <CollidablePair> pairs = new List <CollidablePair>();
int splitChunks = 0, proccesChunks = 0;
for (int m = 0; m < final.Count; m++)
{
splitChunks++;
if (final[m].Count <= 2) //Two markers per box
{
continue; //there is only one box in this segment
}
proccesChunks++;
/*
* final[m].Sort((x, y) => x.position.X.CompareTo(y.position.X));
* Prune(final[m], out var markersy);
*
* markersy.Sort((x, y) => x.position.Y.CompareTo(y.position.Y));
* Prune(markersy, out var markersz);
*
* markersz.Sort((x, y) => x.position.Z.CompareTo(y.position.Z));
* Prune(markersz, out var markersfinal);
*/
var markersfinal = final[m];
AABB last = null;
List <AABB> open = new List <AABB>();
for (int j = 0; j < markersfinal.Count; j++)
{
if (markersfinal[j] is MinAABB)
{
last = ((MinAABB)markersfinal[j]).a;
open.Add(last);
}
else
{
if (markersfinal[j].a == last)
{
open.Remove(last);
}
foreach (var aa in open)
{
if (aa.root != markersfinal[j].a.root)
{
RigidbodyHandle a = (RigidbodyHandle)aa.root;
RigidbodyHandle b = (RigidbodyHandle)markersfinal[j].a.root;
pairs.Add(new CollidablePair {
a = a, b = b
});
}
}
if (open.Contains(markersfinal[j].a))
{
open.Remove(markersfinal[j].a);
}
}
}
}
return(pairs);
}
internal static Span <RigidbodyState> GetRBInterpolatedState(RigidbodyHandle handle)
{
return(interpolatedState.GetRef(handle.handle));
}
internal static Span <RigidbodyState> GetRBState(RigidbodyHandle handle)
{
return(editState.GetRef(handle.handle));
}
