unsafe void SolveSingleJoint(JointData *jointData, int numIterations, float timestep,
ref MotionVelocity velocityA, ref MotionVelocity velocityB, ref MotionData motionA, ref MotionData motionB, out BlockStream jacobiansOut)
{
Solver.StepInput stepInput = new Solver.StepInput
{
IsLastIteration = false,
InvNumSolverIterations = 1.0f / numIterations,
Timestep = timestep,
InvTimestep = timestep > 0.0f ? 1.0f / timestep : 0.0f
};
// Build jacobians
jacobiansOut = new BlockStream(1, 0);
{
BlockStream.Writer jacobianWriter = jacobiansOut;
jacobianWriter.BeginForEachIndex(0);
Solver.BuildJointJacobian(jointData, new BodyIndexPair(), velocityA, velocityB, motionA, motionB, timestep, numIterations, ref jacobianWriter);
jacobianWriter.EndForEachIndex();
}
BlockStream.Writer eventWriter = new BlockStream.Writer(); // no events expected
// Solve the joint
for (int iIteration = 0; iIteration < numIterations; iIteration++)
{
stepInput.IsLastIteration = (iIteration == numIterations - 1);
BlockStream.Reader jacobianReader = jacobiansOut;
JacobianIterator jacIterator = new JacobianIterator(jacobianReader, 0);
while (jacIterator.HasJacobiansLeft())
{
ref JacobianHeader header = ref jacIterator.ReadJacobianHeader();
header.Solve(ref velocityA, ref velocityB, stepInput, ref eventWriter, ref eventWriter);
}
}
public Enumerator(BlockStream stream)
{
m_Reader = stream.IsCreated ? stream : new BlockStream.Reader();
m_CurrentWorkItem = 0;
m_NumWorkItems = stream.IsCreated ? stream.ForEachCount : 0;
m_Current = default(TriggerEvent);
AdvanceReader();
}
public void ReadWithoutBegin()
{
BlockStream stream;
CreateBlockStream1And2Int(out stream);
BlockStream.Reader reader = stream;
Assert.Throws <ArgumentException>(() => reader.Read <int>());
stream.Dispose();
}
public void JacobianIteratorHasJacobiansLeftTest()
{
var jacobianStream = new BlockStream(1, 0x01234567);
BlockStream.Reader jacobianStreamReader = jacobianStream;
int workItemIndex = 0;
var jacIterator = new JacobianIterator(jacobianStreamReader, workItemIndex);
Assert.IsFalse(jacIterator.HasJacobiansLeft());
jacobianStream.Dispose();
}
public void OutOfBoundsRead()
{
BlockStream stream;
CreateBlockStream1And2Int(out stream);
BlockStream.Reader reader = stream;
reader.BeginForEachIndex(0);
Assert.Throws <ArgumentException>(() => reader.Read <long>());
stream.Dispose();
}
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 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 Iterator(Contacts contacts)
{
m_SimulationType = contacts.m_SimulationType;
m_ContactReader = new BlockStream.Reader(contacts.m_ContactStream);
m_CurrentWorkItem = 0;
m_MaxNumWorkItems = contacts.m_MaxNumWorkItems;
while (m_ContactReader.RemainingItemCount == 0 && m_CurrentWorkItem < contacts.m_MaxNumWorkItems)
{
m_ContactReader.BeginForEachIndex(m_CurrentWorkItem);
m_CurrentWorkItem++;
}
m_LastHeader = new ContactHeader();
m_NumPointsLeft = 0;
}
private static void BuildContactJacobian(
int contactPointIndex,
float3 normal,
MTransform worldFromA,
MTransform worldFromB,
float invTimestep,
MotionVelocity velocityA,
MotionVelocity velocityB,
float sumInvMass,
float maxDepenetrationVelocity,
ref JacobianHeader jacobianHeader,
ref float3 centerA,
ref float3 centerB,
ref BlockStream.Reader contactReader)
{
ref ContactJacAngAndVelToReachCp jacAngular = ref jacobianHeader.AccessAngularJacobian(contactPointIndex);
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 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 Jacobians(BlockStream.Reader jacobianStreamReader, int workItemIndex)
{
m_IsCreated = true;
m_JacobianStreamReader = jacobianStreamReader;
m_WorkItemIndex = workItemIndex;
}
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;
}
