public ContactHeader GetNextContactHeader()
{
while (m_NumPointsLeft > 0)
{
m_ContactReader.Read <ContactPoint>();
}
m_LastHeader = m_ContactReader.Read <ContactHeader>();
m_NumPointsLeft = m_LastHeader.NumContacts;
return(m_LastHeader);
}
public void TooManyReads()
{
BlockStream stream;
CreateBlockStream1And2Int(out stream);
BlockStream.Reader reader = stream;
reader.BeginForEachIndex(0);
reader.Read <byte>();
Assert.Throws <ArgumentException>(() => reader.Read <byte>());
stream.Dispose();
}
public bool MoveNext()
{
if (m_Reader.RemainingItemCount > 0)
{
int currentSize = m_Reader.Read <int>();
AdvanceReader();
unsafe
{
m_Current = (CollisionEvent *)m_Reader.Read(currentSize);
}
AdvanceReader();
return(true);
}
return(false);
}
public bool MoveNext()
{
if (m_Reader.RemainingItemCount > 0)
{
m_Current = m_Reader.Read <CollisionEvent>();
AdvanceReader();
return(true);
}
return(false);
}
public bool MoveNext()
{
if (m_Reader.RemainingItemCount > 0)
{
Current = m_Reader.Read <TriggerEvent>();
AdvanceReader();
return(true);
}
return(false);
}
public void ReadWithoutBegin()
{
BlockStream stream;
CreateBlockStream1And2Int(out stream);
BlockStream.Reader reader = stream;
Assert.Throws <ArgumentException>(() => reader.Read <int>());
stream.Dispose();
}
public void OutOfBoundsRead()
{
BlockStream stream;
CreateBlockStream1And2Int(out stream);
BlockStream.Reader reader = stream;
reader.BeginForEachIndex(0);
Assert.Throws <ArgumentException>(() => reader.Read <long>());
stream.Dispose();
}
private void Draw()
{
for (int i = 0; i < m_DebugStreams.Count; i++)
{
BlockStream.Reader reader = m_DebugStreams[i];
for (int j = 0; j != reader.ForEachCount; j++)
{
reader.BeginForEachIndex(j);
while (reader.RemainingItemCount != 0)
{
switch (reader.Read <Type>())
{
case Type.Point: reader.Read <Point>().Draw(); break;
case Type.Line: reader.Read <Line>().Draw(); break;
case Type.Arrow: reader.Read <Line>().DrawArrow(); break;
case Type.Plane: reader.Read <Line>().DrawPlane(); break;
case Type.Circle: reader.Read <Line>().DrawCircle(); break;
case Type.Arc: reader.Read <Arc>().Draw(); break;
case Type.Cone: reader.Read <Cone>().Draw(); break;
case Type.Text: reader.Read <Text>().Draw(ref reader); break;
case Type.Box: reader.Read <Box>().Draw(); break;
default: return; // unknown type
}
}
reader.EndForEachIndex();
}
}
}
public void Execute(int index)
{
int count = Reader.BeginForEachIndex(index);
Assert.AreEqual(count, index);
for (int i = 0; i != index; i++)
{
Assert.AreEqual(index - i, Reader.RemainingItemCount);
var peekedValue = Reader.Peek <int>();
var value = Reader.Read <int>();
Assert.AreEqual(i, value);
Assert.AreEqual(i, peekedValue);
}
}
public void Draw(ref BlockStream.Reader reader)
{
// Read string data.
char[] stringBuf = new char[Length];
for (int i = 0; i < Length; i++)
{
stringBuf[i] = reader.Read <char>();
}
GUIStyle style = new GUIStyle();
style.normal.textColor = Color;
#if UNITY_EDITOR
Handles.Label(X, new string(stringBuf), style);
#endif
}
public void IncorrectTypedReads()
{
var stream = new BlockStream(1);
BlockStream.Writer writer = stream;
writer.BeginForEachIndex(0);
writer.Write <int>(5);
writer.EndForEachIndex();
BlockStream.Reader reader = stream;
reader.BeginForEachIndex(0);
Assert.Throws <UnityEngine.Assertions.AssertionException>(() => reader.Read <float>());
stream.Dispose();
}
public void Execute()
{
int index = 0;
int maxIndex = DeferredImpulseReader.ForEachCount;
DeferredImpulseReader.BeginForEachIndex(index++);
while (DeferredImpulseReader.RemainingItemCount == 0 && index < maxIndex)
{
DeferredImpulseReader.BeginForEachIndex(index++);
}
while (DeferredImpulseReader.RemainingItemCount > 0)
{
// Read the data
var impulse = DeferredImpulseReader.Read <DeferredCharacterControllerImpulse>();
while (DeferredImpulseReader.RemainingItemCount == 0 && index < maxIndex)
{
DeferredImpulseReader.BeginForEachIndex(index++);
}
PhysicsVelocity pv = PhysicsVelocityData[impulse.Entity];
PhysicsMass pm = PhysicsMassData[impulse.Entity];
Translation t = TranslationData[impulse.Entity];
Rotation r = RotationData[impulse.Entity];
// Don't apply on kinematic bodies
if (pm.InverseMass > 0.0f)
{
// Apply impulse
pv.ApplyImpulse(pm, t, r, impulse.Impulse, impulse.Point);
// Write back
PhysicsVelocityData[impulse.Entity] = pv;
}
}
}
public unsafe void OverlapTaskFilteringTest([Values(2, 10, 33, 100)] int elementCount)
{
elementCount *= 2;
int numNodes = elementCount + Constants.MaxNumTreeBranches;
var points = new NativeArray <PointAndIndex>(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var aabbs = new NativeArray <Aabb>(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var bodyFilters = new NativeArray <CollisionFilter>(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
InitInputWithCopyArrays(points, aabbs, bodyFilters);
var nodes = new NativeArray <Node>(numNodes, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
Node *nodesPtr = (Node *)nodes.GetUnsafePtr();
var seenUnfiltered = new HashSet <BodyIndexPair>();
{
var bvhUnfiltered = new BoundingVolumeHierarchy(nodes);
bvhUnfiltered.Build(points, aabbs, out int numNodesOut);
bvhUnfiltered.CheckIntegrity();
EverythingWriter pairWriter = new EverythingWriter {
SeenPairs = seenUnfiltered
};
BoundingVolumeHierarchy.TreeOverlap(ref pairWriter, nodesPtr, nodesPtr);
}
var nodeFilters = new NativeArray <CollisionFilter>(numNodes, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var bvhFiltered = new BoundingVolumeHierarchy(nodes, nodeFilters);
int numNodesFilteredTree;
bvhFiltered.Build(points, aabbs, out numNodesFilteredTree);
bvhFiltered.BuildCombinedCollisionFilter(bodyFilters, 0, numNodesFilteredTree - 1);
var filteredCollisionPairs = new BlockStream(1, 0xec87b613);
BlockStream.Writer filteredPairWriter = filteredCollisionPairs;
filteredPairWriter.BeginForEachIndex(0);
CollisionFilter *bodyFiltersPtr = (CollisionFilter *)bodyFilters.GetUnsafePtr();
var bufferedPairs = new Broadphase.BodyPairWriter(&filteredPairWriter, bodyFiltersPtr);
CollisionFilter *nodeFiltersPtr = (CollisionFilter *)nodeFilters.GetUnsafePtr();
BoundingVolumeHierarchy.TreeOverlap(ref bufferedPairs, nodesPtr, nodesPtr, nodeFiltersPtr, nodeFiltersPtr);
bufferedPairs.Close();
filteredPairWriter.EndForEachIndex();
BlockStream.Reader filteredPairReader = filteredCollisionPairs;
filteredPairReader.BeginForEachIndex(0);
// Check that every pair in our filtered set also appears in the unfiltered set
while (filteredPairReader.RemainingItemCount > 0)
{
var pair = filteredPairReader.Read <BodyIndexPair>();
Assert.IsTrue(seenUnfiltered.Contains(pair));
seenUnfiltered.Remove(pair); // Remove the pair
}
// Pairs were removed, so the only remaining ones should be filtered
foreach (BodyIndexPair pair in seenUnfiltered)
{
bool shouldCollide = CollisionFilter.IsCollisionEnabled(bodyFilters[pair.BodyAIndex], bodyFilters[pair.BodyBIndex]);
Assert.IsFalse(shouldCollide);
}
nodeFilters.Dispose();
nodes.Dispose();
bodyFilters.Dispose();
aabbs.Dispose();
points.Dispose();
filteredCollisionPairs.Dispose();
}
public unsafe void ManifoldQueryTest()
{
const uint seed = 0x98765432;
Random rnd = new Random(seed);
int numWorlds = 1000;
uint dbgWorld = 0;
if (dbgWorld > 0)
{
numWorlds = 1;
}
for (int iWorld = 0; iWorld < numWorlds; iWorld++)
{
// Save state to repro this query without doing everything that came before it
if (dbgWorld > 0)
{
rnd.state = dbgWorld;
}
uint worldState = rnd.state;
Physics.PhysicsWorld world = TestUtils.GenerateRandomWorld(ref rnd, rnd.NextInt(1, 20), 3.0f);
// Manifold test
// TODO would be nice if we could change the world collision tolerance
for (int iBodyA = 0; iBodyA < world.NumBodies; iBodyA++)
{
for (int iBodyB = iBodyA + 1; iBodyB < world.NumBodies; iBodyB++)
{
Physics.RigidBody bodyA = world.Bodies[iBodyA];
Physics.RigidBody bodyB = world.Bodies[iBodyB];
if (bodyA.Collider->Type == ColliderType.Mesh && bodyB.Collider->Type == ColliderType.Mesh)
{
continue; // TODO - no mesh-mesh manifold support yet
}
// Build manifolds
BlockStream contacts = new BlockStream(1, 0, Allocator.Temp);
BlockStream.Writer contactWriter = contacts;
contactWriter.BeginForEachIndex(0);
ManifoldQueries.BodyBody(ref world, new BodyIndexPair {
BodyAIndex = iBodyA, BodyBIndex = iBodyB
}, 1.0f, ref contactWriter);
contactWriter.EndForEachIndex();
// Read each manifold
BlockStream.Reader contactReader = contacts;
contactReader.BeginForEachIndex(0);
int manifoldIndex = 0;
while (contactReader.RemainingItemCount > 0)
{
string failureMessage = iWorld + " (" + worldState + ") " + iBodyA + " vs " + iBodyB + " #" + manifoldIndex;
manifoldIndex++;
// Read the manifold header
ContactHeader header = contactReader.Read <ContactHeader>();
ConvexConvexManifoldQueries.Manifold manifold = new ConvexConvexManifoldQueries.Manifold();
manifold.NumContacts = header.NumContacts;
manifold.Normal = header.Normal;
// Get the leaf shapes
ChildCollider leafA, leafB;
{
Collider.GetLeafCollider(bodyA.Collider, bodyA.WorldFromBody, header.ColliderKeys.ColliderKeyA, out leafA);
Collider.GetLeafCollider(bodyB.Collider, bodyB.WorldFromBody, header.ColliderKeys.ColliderKeyB, out leafB);
}
// Read each contact point
int minIndex = 0;
for (int iContact = 0; iContact < header.NumContacts; iContact++)
{
// Read the contact and find the closest
ContactPoint contact = contactReader.Read <ContactPoint>();
manifold[iContact] = contact;
if (contact.Distance < manifold[minIndex].Distance)
{
minIndex = iContact;
}
// Check that the contact point is on or inside the shape
CheckPointOnSurface(ref leafA, contact.Position + manifold.Normal * contact.Distance, failureMessage + " contact " + iContact + " leaf A");
CheckPointOnSurface(ref leafB, contact.Position, failureMessage + " contact " + iContact + " leaf B");
}
// Check the closest point
{
ContactPoint closestPoint = manifold[minIndex];
RigidTransform aFromWorld = math.inverse(leafA.TransformFromChild);
DistanceQueries.Result result = new DistanceQueries.Result
{
PositionOnAinA = math.transform(aFromWorld, closestPoint.Position + manifold.Normal * closestPoint.Distance),
NormalInA = math.mul(aFromWorld.rot, manifold.Normal),
Distance = closestPoint.Distance
};
MTransform aFromB = new MTransform(math.mul(aFromWorld, leafB.TransformFromChild));
float referenceDistance = DistanceQueries.ConvexConvex(leafA.Collider, leafB.Collider, aFromB).Distance;
ValidateDistanceResult(result, ref ((ConvexCollider *)leafA.Collider)->ConvexHull, ref ((ConvexCollider *)leafB.Collider)->ConvexHull, aFromB, referenceDistance, failureMessage + " closest point");
}
// Check that the manifold is flat
CheckManifoldFlat(ref manifold, manifold.Normal, failureMessage + ": non-flat A");
CheckManifoldFlat(ref manifold, float3.zero, failureMessage + ": non-flat B");
}
contacts.Dispose();
}
}
world.Dispose(); // TODO leaking memory if the test fails
}
}
// Update is called once per frame
private void Update()
{
Vector3 imageCenter = transform.TransformPoint(new Vector3(0, 0, -ImagePlane));
if (ExpectingResults)
{
BlockStream.Reader reader = lastResults.PixelData;
for (int i = 0; i < lastResults.PixelData.ForEachCount; i++)
{
reader.BeginForEachIndex(i);
while (reader.RemainingItemCount > 0)
{
int x = reader.Read <int>();
int y = reader.Read <int>();
Color c = reader.Read <Color>();
blasterTexture.SetPixel(x, y, c);
}
reader.EndForEachIndex();
}
blasterTexture.Apply();
lastResults.PixelData.Dispose();
ExpectingResults = false;
}
if (BasePhysicsDemo.DefaultWorld == null)
{
return;
}
RayTracerSystem rbs = BasePhysicsDemo.DefaultWorld.GetExistingSystem <RayTracerSystem>();
if (rbs == null || !rbs.IsEnabled)
{
return;
}
Vector3 lightDir = new Vector3(0, 0, -1);
GameObject sceneLight = GameObject.Find("Directional Light");
if (sceneLight != null)
{
lightDir = sceneLight.transform.rotation * lightDir;
}
Vector3 up = transform.rotation * new Vector3(0, 1, 0);
Vector3 right = transform.rotation * new Vector3(1, 0, 0);
lastResults = rbs.AddRequest(new RayTracerSystem.RayRequest
{
PinHole = transform.position,
ImageCenter = imageCenter,
Up = up,
Right = right,
LightDir = lightDir,
RayLength = RayLength,
PlaneHalfExtents = planeHalfExtents,
AmbientLight = AmbientLight,
ImageResolution = ImageRes,
AlternateKeys = AlternateKeys,
CastSphere = CastSphere,
Shadows = Shadows,
CollisionFilter = CollisionFilter.Default
});
ExpectingResults = true;
}
