static void CreateBlockStream1And2Int(out BlockStream stream) { stream = new BlockStream(2, 0x9b98651c); BlockStream.Writer writer = stream; writer.BeginForEachIndex(0); writer.Write(0); writer.EndForEachIndex(); writer.BeginForEachIndex(1); writer.Write(1); writer.Write(2); writer.EndForEachIndex(); }
private unsafe void WriteEvent(LowLevel.CollisionEvent collisionEvent, ref BlockStream.Writer collisionEventWriter) { int numContactPoints = collisionEvent.NumNarrowPhaseContactPoints; int size = UnsafeUtility.SizeOf <LowLevel.CollisionEvent>() + numContactPoints * UnsafeUtility.SizeOf <ContactPoint>(); collisionEventWriter.Write(size); byte *eventPtr = collisionEventWriter.Allocate(size); ref LowLevel.CollisionEvent eventRef = ref UnsafeUtilityEx.AsRef <LowLevel.CollisionEvent>(eventPtr);
public void ReadCollisionEvents() { // Allocate a block stream for up to 10 parallel writes BlockStream collisionEventStream = new BlockStream(10, 0xabbaabba); // Do a couple of writes to different forEach indices int writeCount = 0; { BlockStream.Writer collisionEventWriter = collisionEventStream; collisionEventWriter.BeginForEachIndex(1); collisionEventWriter.Write(new CollisionEvent()); writeCount++; collisionEventWriter.Write(new CollisionEvent()); writeCount++; collisionEventWriter.EndForEachIndex(); collisionEventWriter.BeginForEachIndex(3); collisionEventWriter.Write(new CollisionEvent()); writeCount++; collisionEventWriter.EndForEachIndex(); collisionEventWriter.BeginForEachIndex(5); collisionEventWriter.Write(new CollisionEvent()); writeCount++; collisionEventWriter.Write(new CollisionEvent()); writeCount++; collisionEventWriter.EndForEachIndex(); collisionEventWriter.BeginForEachIndex(7); collisionEventWriter.Write(new CollisionEvent()); writeCount++; collisionEventWriter.EndForEachIndex(); collisionEventWriter.BeginForEachIndex(9); collisionEventWriter.Write(new CollisionEvent()); writeCount++; collisionEventWriter.Write(new CollisionEvent()); writeCount++; collisionEventWriter.EndForEachIndex(); } // Iterate over written events and make sure they are all read CollisionEvents collisionEvents = new CollisionEvents(collisionEventStream); int readCount = 0; foreach (var collisionEvent in collisionEvents) { readCount++; } Assert.IsTrue(readCount == writeCount); // Cleanup var disposeJob = collisionEventStream.ScheduleDispose(default);
public void WriteWithoutBegin() { var stream = new BlockStream(1, 0x9b98651c); BlockStream.Writer writer = stream; Assert.Throws <ArgumentException>(() => writer.Write(5)); stream.Dispose(); }
public void Execute(int index) { Writer.BeginForEachIndex(index); for (int i = 0; i != index; i++) { Writer.Write(i); } Writer.EndForEachIndex(); }
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 unsafe void Execute(int index) { Results.BeginForEachIndex(index); int numRows = (Request.ImageResolution + Results.ForEachCount - 1) / Results.ForEachCount; const float sphereRadius = 0.005f; BlobAssetReference <Collider> sphere; if (Request.CastSphere) { sphere = SphereCollider.Create(float3.zero, sphereRadius, Request.CollisionFilter); } for (int yCoord = index * numRows; yCoord < math.min(Request.ImageResolution, (index + 1) * numRows); yCoord++) { for (int xCoord = 0; xCoord < Request.ImageResolution; xCoord++) { float xFrac = 2.0f * ((xCoord / (float)Request.ImageResolution) - 0.5f); float yFrac = 2.0f * ((yCoord / (float)Request.ImageResolution) - 0.5f); float3 targetImagePlane = Request.ImageCenter + Request.Up * Request.PlaneHalfExtents * yFrac + Request.Right * Request.PlaneHalfExtents * xFrac; float3 rayDir = Request.RayLength * (Request.PinHole - targetImagePlane); RaycastHit hit; bool hasHit; if (Request.CastSphere) { var input = new ColliderCastInput { Collider = (Collider *)sphere.GetUnsafePtr(), Orientation = quaternion.identity, Start = Request.PinHole, End = Request.PinHole + rayDir }; hasHit = World.CastCollider(input, out ColliderCastHit colliderHit); hit = new RaycastHit { Fraction = colliderHit.Fraction, Position = colliderHit.Position, SurfaceNormal = colliderHit.SurfaceNormal, RigidBodyIndex = colliderHit.RigidBodyIndex, ColliderKey = colliderHit.ColliderKey }; } else { var rayCastInput = new RaycastInput { Start = Request.PinHole, End = Request.PinHole + rayDir, Filter = Request.CollisionFilter }; hasHit = World.CastRay(rayCastInput, out hit); } Color hitColor = Color.black; if (hasHit) { if (hit.RigidBodyIndex < NumDynamicBodies) { hitColor = Color.yellow; } else { hitColor = Color.grey; } // Lighten alternate keys if (Request.AlternateKeys && !hit.ColliderKey.Equals(ColliderKey.Empty)) { Collider *collider = World.Bodies[hit.RigidBodyIndex].Collider; hit.ColliderKey.PopSubKey(collider->NumColliderKeyBits, out uint key); if (key % 2 == 0) { Color.RGBToHSV(hitColor, out float h, out float s, out float v); hitColor = Color.HSVToRGB(h, s, v + 0.25f); } } if (Request.Shadows) { float3 hitPos = Request.PinHole + rayDir * hit.Fraction + hit.SurfaceNormal * 0.001f; bool shadowHit = false; if (Request.CastSphere) { var start = hitPos + hit.SurfaceNormal * sphereRadius; var input = new ColliderCastInput { Collider = (Collider *)sphere.GetUnsafePtr(), Orientation = quaternion.identity, Start = start, End = start + (Request.LightDir * Request.RayLength), }; ColliderCastHit colliderHit; shadowHit = World.CastCollider(input, out colliderHit); } else { var rayCastInput = new RaycastInput { Start = hitPos, End = hitPos + (Request.LightDir * Request.RayLength), Filter = Request.CollisionFilter }; RaycastHit shadowOutput; shadowHit = World.CastRay(rayCastInput, out shadowOutput); } if (shadowHit) { hitColor *= 0.4f; } } } float lighting = math.min(1.0f, math.max(Request.AmbientLight, Vector3.Dot(hit.SurfaceNormal, Request.LightDir))); Results.Write(xCoord); Results.Write(yCoord); Results.Write(hitColor * lighting); } } Results.EndForEachIndex(); }
private static unsafe void ResolveContacts(PhysicsWorld world, float deltaTime, float3 gravity, float tau, float damping, float characterMass, float3 linearVelocity, int numConstraints, ref NativeArray <SurfaceConstraintInfo> constraints, ref BlockStream.Writer deferredImpulseWriter) { for (int i = 0; i < numConstraints; i++) { SurfaceConstraintInfo constraint = constraints[i]; int rigidBodyIndex = constraint.RigidBodyIndex; if (rigidBodyIndex < 0 || rigidBodyIndex >= world.NumDynamicBodies) { // Invalid and static bodies should be skipped continue; } RigidBody body = world.Bodies[rigidBodyIndex]; float3 pointRelVel = world.GetLinearVelocity(rigidBodyIndex, constraint.HitPosition); pointRelVel -= linearVelocity; float projectedVelocity = math.dot(pointRelVel, constraint.Plane.Normal); // Required velocity change float deltaVelocity = -projectedVelocity * damping; float distance = constraint.Plane.Distance; if (distance < 0.0f) { deltaVelocity += (distance / deltaTime) * tau; } // Calculate impulse MotionVelocity mv = world.MotionVelocities[rigidBodyIndex]; float3 impulse = float3.zero; if (deltaVelocity < 0.0f) { // Impulse magnitude float impulseMagnitude = 0.0f; { float3 arm = constraint.HitPosition - (body.WorldFromBody.pos + body.Collider->MassProperties.MassDistribution.Transform.pos); float3 jacAng = math.cross(arm, constraint.Plane.Normal); float3 armC = jacAng * mv.InverseInertiaAndMass.xyz; float objectMassInv = math.dot(armC, jacAng); objectMassInv += mv.InverseInertiaAndMass.w; impulseMagnitude = deltaVelocity / objectMassInv; } impulse = impulseMagnitude * constraint.Plane.Normal; } // Add gravity { // Effect of gravity on character velocity in the normal direction float3 charVelDown = gravity * deltaTime; float relVelN = math.dot(charVelDown, constraint.Plane.Normal); // Subtract separation velocity if separating contact { bool isSeparatingContact = projectedVelocity < 0.0f; float newRelVelN = relVelN - projectedVelocity; relVelN = math.select(relVelN, newRelVelN, isSeparatingContact); } // If resulting velocity is negative, an impulse is applied to stop the character // from falling into the body { float3 newImpulse = impulse; newImpulse += relVelN * characterMass * constraint.Plane.Normal; impulse = math.select(impulse, newImpulse, relVelN < 0.0f); } } // Store impulse deferredImpulseWriter.Write( new DeferredCharacterControllerImpulse() { Entity = body.Entity, Impulse = impulse, Point = constraint.HitPosition }); } }
private static unsafe void CalculateAndStoreDeferredImpulses( CharacterControllerStepInput stepInput, float characterMass, float3 linearVelocity, int numConstraints, ref NativeArray <SurfaceConstraintInfo> constraints, ref BlockStream.Writer deferredImpulseWriter) { PhysicsWorld world = stepInput.World; for (int i = 0; i < numConstraints; i++) { SurfaceConstraintInfo constraint = constraints[i]; int rigidBodyIndex = constraint.RigidBodyIndex; if (rigidBodyIndex < 0 || rigidBodyIndex >= world.NumDynamicBodies) { // Invalid and static bodies should be skipped continue; } RigidBody body = world.Bodies[rigidBodyIndex]; float3 pointRelVel = world.GetLinearVelocity(rigidBodyIndex, constraint.HitPosition); pointRelVel -= linearVelocity; float projectedVelocity = math.dot(pointRelVel, constraint.Plane.Normal); // Required velocity change float deltaVelocity = -projectedVelocity * stepInput.Damping; float distance = constraint.Plane.Distance; if (distance < 0.0f) { deltaVelocity += (distance / stepInput.DeltaTime) * stepInput.Tau; } // Calculate impulse MotionVelocity mv = world.MotionVelocities[rigidBodyIndex]; float3 impulse = float3.zero; if (deltaVelocity < 0.0f) { // Impulse magnitude float impulseMagnitude = 0.0f; { float objectMassInv = GetInvMassAtPoint(constraint.HitPosition, constraint.Plane.Normal, body, mv); impulseMagnitude = deltaVelocity / objectMassInv; } impulse = impulseMagnitude * constraint.Plane.Normal; } // Add gravity { // Effect of gravity on character velocity in the normal direction float3 charVelDown = stepInput.Gravity * stepInput.DeltaTime; float relVelN = math.dot(charVelDown, constraint.Plane.Normal); // Subtract separation velocity if separating contact { bool isSeparatingContact = projectedVelocity < 0.0f; float newRelVelN = relVelN - projectedVelocity; relVelN = math.select(relVelN, newRelVelN, isSeparatingContact); } // If resulting velocity is negative, an impulse is applied to stop the character // from falling into the body { float3 newImpulse = impulse; newImpulse += relVelN * characterMass * constraint.Plane.Normal; impulse = math.select(impulse, newImpulse, relVelN < 0.0f); } } // Store impulse deferredImpulseWriter.Write( new DeferredCharacterControllerImpulse() { Entity = body.Entity, Impulse = impulse, Point = constraint.HitPosition }); } }
private static unsafe void ConvexConvex( Context context, ColliderKeyPair colliderKeys, Collider *convexColliderA, Collider *convexColliderB, MTransform worldFromA, MTransform worldFromB, float maxDistance, bool flipped, ref BlockStream.Writer contactWriter) { MTransform aFromB = Mul(Inverse(worldFromA), worldFromB); ConvexConvexManifoldQueries.Manifold contactManifold; switch (convexColliderA->Type) { case ColliderType.Sphere: switch (convexColliderB->Type) { case ColliderType.Sphere: ConvexConvexManifoldQueries.SphereSphere( (SphereCollider *)convexColliderA, (SphereCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Capsule: ConvexConvexManifoldQueries.CapsuleSphere( (CapsuleCollider *)convexColliderB, (SphereCollider *)convexColliderA, worldFromB, Inverse(aFromB), maxDistance, out contactManifold); flipped = !flipped; break; case ColliderType.Triangle: ConvexConvexManifoldQueries.TriangleSphere( (PolygonCollider *)convexColliderB, (SphereCollider *)convexColliderA, worldFromB, Inverse(aFromB), maxDistance, out contactManifold); flipped = !flipped; break; case ColliderType.Box: ConvexConvexManifoldQueries.BoxSphere( (BoxCollider *)convexColliderB, (SphereCollider *)convexColliderA, worldFromB, Inverse(aFromB), maxDistance, out contactManifold); flipped = !flipped; break; case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: ConvexConvexManifoldQueries.ConvexConvex( ref ((SphereCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull, worldFromA, aFromB, maxDistance, out contactManifold); break; default: throw new NotImplementedException(); } break; case ColliderType.Box: switch (convexColliderB->Type) { case ColliderType.Sphere: ConvexConvexManifoldQueries.BoxSphere( (BoxCollider *)convexColliderA, (SphereCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Triangle: ConvexConvexManifoldQueries.BoxTriangle( (BoxCollider *)convexColliderA, (PolygonCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Box: ConvexConvexManifoldQueries.BoxBox( (BoxCollider *)convexColliderA, (BoxCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Capsule: case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: ConvexConvexManifoldQueries.ConvexConvex( ref ((BoxCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull, worldFromA, aFromB, maxDistance, out contactManifold); break; default: throw new NotImplementedException(); } break; case ColliderType.Capsule: switch (convexColliderB->Type) { case ColliderType.Sphere: ConvexConvexManifoldQueries.CapsuleSphere( (CapsuleCollider *)convexColliderA, (SphereCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Capsule: ConvexConvexManifoldQueries.CapsuleCapsule( (CapsuleCollider *)convexColliderA, (CapsuleCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Triangle: ConvexConvexManifoldQueries.CapsuleTriangle( (CapsuleCollider *)convexColliderA, (PolygonCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Quad: case ColliderType.Box: case ColliderType.Cylinder: case ColliderType.Convex: ConvexConvexManifoldQueries.ConvexConvex( ref ((CapsuleCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull, worldFromA, aFromB, maxDistance, out contactManifold); break; default: throw new NotImplementedException(); } break; case ColliderType.Triangle: switch (convexColliderB->Type) { case ColliderType.Sphere: ConvexConvexManifoldQueries.TriangleSphere( (PolygonCollider *)convexColliderA, (SphereCollider *)convexColliderB, worldFromA, aFromB, maxDistance, out contactManifold); break; case ColliderType.Capsule: ConvexConvexManifoldQueries.CapsuleTriangle( (CapsuleCollider *)convexColliderB, (PolygonCollider *)convexColliderA, worldFromB, Inverse(aFromB), maxDistance, out contactManifold); flipped = !flipped; break; case ColliderType.Box: ConvexConvexManifoldQueries.BoxTriangle( (BoxCollider *)convexColliderB, (PolygonCollider *)convexColliderA, worldFromB, Inverse(aFromB), maxDistance, out contactManifold); flipped = !flipped; break; case ColliderType.Triangle: case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: ConvexConvexManifoldQueries.ConvexConvex( ref ((PolygonCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull, worldFromA, aFromB, maxDistance, out contactManifold); break; default: throw new NotImplementedException(); } break; case ColliderType.Quad: case ColliderType.Cylinder: case ColliderType.Convex: ConvexConvexManifoldQueries.ConvexConvex( ref ((ConvexCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull, worldFromA, aFromB, maxDistance, out contactManifold); break; default: throw new NotImplementedException(); } // Write results to stream if (contactManifold.NumContacts > 0) { if (flipped) { contactManifold.Flip(); } var header = new ContactHeader { BodyPair = context.BodyIndices, BodyCustomDatas = context.BodyCustomDatas, NumContacts = contactManifold.NumContacts, Normal = contactManifold.Normal, ColliderKeys = colliderKeys }; // Apply materials { Material materialA = ((ConvexColliderHeader *)convexColliderA)->Material; Material materialB = ((ConvexColliderHeader *)convexColliderB)->Material; Material.MaterialFlags combinedFlags = materialA.Flags | materialB.Flags; if ((combinedFlags & Material.MaterialFlags.IsTrigger) != 0) { header.JacobianFlags |= JacobianFlags.IsTrigger; } else { if ((combinedFlags & Material.MaterialFlags.EnableCollisionEvents) != 0) { header.JacobianFlags |= JacobianFlags.EnableCollisionEvents; } if ((combinedFlags & Material.MaterialFlags.EnableMassFactors) != 0) { header.JacobianFlags |= JacobianFlags.EnableMassFactors; } if ((combinedFlags & Material.MaterialFlags.EnableSurfaceVelocity) != 0) { header.JacobianFlags |= JacobianFlags.EnableSurfaceVelocity; } if ((combinedFlags & Material.MaterialFlags.EnableMaxImpulse) != 0) { header.JacobianFlags |= JacobianFlags.EnableMaxImpulse; } header.CoefficientOfFriction = Material.GetCombinedFriction(materialA, materialB); header.CoefficientOfRestitution = Material.GetCombinedRestitution(materialA, materialB); } } contactWriter.Write(header); for (int contactIndex = 0; contactIndex < header.NumContacts; contactIndex++) { contactWriter.Write(contactManifold[contactIndex]); } } }
private static void WriteManifold(ConvexConvexManifoldQueries.Manifold manifold, Context context, ColliderKeyPair colliderKeys, Material materialA, Material materialB, bool flipped, ref BlockStream.Writer contactWriter) { // Write results to stream if (manifold.NumContacts > 0) { if (flipped) { manifold.Flip(); } var header = new ContactHeader { BodyPair = context.BodyIndices, BodyCustomTags = context.BodyCustomTags, NumContacts = manifold.NumContacts, Normal = manifold.Normal, ColliderKeys = colliderKeys }; // Apply materials { Material.MaterialFlags combinedFlags = materialA.Flags | materialB.Flags; if ((combinedFlags & Material.MaterialFlags.IsTrigger) != 0) { header.JacobianFlags |= JacobianFlags.IsTrigger; } else { if ((combinedFlags & Material.MaterialFlags.EnableCollisionEvents) != 0) { header.JacobianFlags |= JacobianFlags.EnableCollisionEvents; } if ((combinedFlags & Material.MaterialFlags.EnableMassFactors) != 0) { header.JacobianFlags |= JacobianFlags.EnableMassFactors; } if ((combinedFlags & Material.MaterialFlags.EnableSurfaceVelocity) != 0) { header.JacobianFlags |= JacobianFlags.EnableSurfaceVelocity; } header.CoefficientOfFriction = Material.GetCombinedFriction(materialA, materialB); header.CoefficientOfRestitution = Material.GetCombinedRestitution(materialA, materialB); } } contactWriter.Write(header); // Group the contact points in 2s (when 4-6 contact points) and 3s (6 or more contact points) // to avoid the order forcing the magnitude of the impulse on one side of the face. // When less than 4 contact points access them in order. int startIndex = 0; int increment = header.NumContacts < 6 ? math.max(header.NumContacts / 2, 1) : (header.NumContacts / 3 + ((header.NumContacts % 3 > 0) ? 1 : 0)); for (int contactIndex = 0; ; contactIndex += increment) { if (contactIndex >= header.NumContacts) { startIndex++; if (startIndex == increment) { break; } contactIndex = startIndex; } contactWriter.Write(manifold[contactIndex]); } } }