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);
Beispiel #2
0
        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();
        }
        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);
                }
            }
Beispiel #4
0
        public void EndForEachIndexWithoutBegin()
        {
            var stream = new BlockStream(1, 0x9b98651c);

            BlockStream.Writer writer = stream;
            Assert.Throws <ArgumentException>(() => writer.EndForEachIndex());

            stream.Dispose();
        }
Beispiel #5
0
 public void Execute(int index)
 {
     Writer.BeginForEachIndex(index);
     for (int i = 0; i != index; i++)
     {
         Writer.Write(i);
     }
     Writer.EndForEachIndex();
 }
Beispiel #6
0
            public unsafe void Execute(int workItemIndex)
            {
                int dispatchPairReadOffset = SolverSchedulerInfo.GetWorkItemReadOffset(workItemIndex, out int numPairsToRead);

                ContactWriter.BeginForEachIndex(workItemIndex);
                JointJacobianWriter.BeginForEachIndex(workItemIndex);

                for (int i = 0; i < numPairsToRead; i++)
                {
                    Scheduler.DispatchPair dispatchPair = PhasedDispatchPairs[dispatchPairReadOffset + i];

                    // Invalid pairs can exist by being disabled by users
                    if (dispatchPair.IsValid)
                    {
                        if (dispatchPair.IsContact)
                        {
                            // Create contact manifolds for this pair of bodies
                            var pair = new BodyIndexPair
                            {
                                BodyAIndex = dispatchPair.BodyAIndex,
                                BodyBIndex = dispatchPair.BodyBIndex
                            };

                            RigidBody rigidBodyA = World.Bodies[pair.BodyAIndex];
                            RigidBody rigidBodyB = World.Bodies[pair.BodyBIndex];

                            MotionVelocity motionVelocityA = pair.BodyAIndex < World.MotionVelocities.Length ?
                                                             World.MotionVelocities[pair.BodyAIndex] : MotionVelocity.Zero;
                            MotionVelocity motionVelocityB = pair.BodyBIndex < World.MotionVelocities.Length ?
                                                             World.MotionVelocities[pair.BodyBIndex] : MotionVelocity.Zero;

                            ManifoldQueries.BodyBody(rigidBodyA, rigidBodyB, motionVelocityA, motionVelocityB,
                                                     World.CollisionWorld.CollisionTolerance, TimeStep, pair, ref ContactWriter);
                        }
                        else
                        {
                            Joint joint = World.Joints[dispatchPair.JointIndex];
                            // Need to fetch the real body indices from the joint, as the scheduler may have reordered them
                            int bodyAIndex = joint.BodyPair.BodyAIndex;
                            int bodyBIndex = joint.BodyPair.BodyBIndex;

                            GetMotion(ref World, bodyAIndex, out MotionVelocity velocityA, out MotionData motionA);
                            GetMotion(ref World, bodyBIndex, out MotionVelocity velocityB, out MotionData motionB);

                            Solver.BuildJointJacobian(joint.JointData, joint.BodyPair, velocityA, velocityB, motionA, motionB, TimeStep, NumIterations, ref JointJacobianWriter);
                        }
                    }
                }

                JointJacobianWriter.EndForEachIndex();
                ContactWriter.EndForEachIndex();
            }
Beispiel #7
0
        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();
        }
Beispiel #8
0
            public unsafe void Execute()
            {
                if (DummyRun)
                {
                    return;
                }

                CollisionPairWriter.BeginForEachIndex(0);

                CollisionFilter *bodyFilters = (CollisionFilter *)Filter.GetUnsafePtr();
                var pairBuffer = new Broadphase.BodyPairWriter((BlockStream.Writer *)UnsafeUtility.AddressOf(ref CollisionPairWriter), bodyFilters);

                Node *nodesPtr = (Node *)Nodes.GetUnsafePtr();

                BoundingVolumeHierarchy.TreeOverlap(ref pairBuffer, nodesPtr, nodesPtr);

                pairBuffer.Close();

                CollisionPairWriter.EndForEachIndex();
            }
            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();
            }
Beispiel #10
0
        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();
        }
Beispiel #11
0
        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
            }
        }
        public unsafe void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex)
        {
            float3 up = math.up();

            var chunkCCData              = chunk.GetNativeArray(CharacterControllerComponentType);
            var chunkCCInternalData      = chunk.GetNativeArray(CharacterControllerInternalType);
            var chunkPhysicsColliderData = chunk.GetNativeArray(PhysicsColliderType);
            var chunkTranslationData     = chunk.GetNativeArray(TranslationType);
            var chunkRotationData        = chunk.GetNativeArray(RotationType);

            DeferredImpulseWriter.BeginForEachIndex(chunkIndex);

            // Maximum number of hits character controller can store in world queries
            const int maxQueryHits = 128;
            var       distanceHits = new NativeArray <DistanceHit>(maxQueryHits, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
            var       castHits     = new NativeArray <ColliderCastHit>(maxQueryHits, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
            var       constraints  = new NativeArray <SurfaceConstraintInfo>(4 * maxQueryHits, Allocator.Temp, NativeArrayOptions.UninitializedMemory);

            for (int i = 0; i < chunk.Count; i++)
            {
                var ccComponentData = chunkCCData[i];
                var ccInternalData  = chunkCCInternalData[i];
                var collider        = chunkPhysicsColliderData[i];
                var position        = chunkTranslationData[i];
                var rotation        = chunkRotationData[i];

                // Collision filter must be valid
                Assert.IsTrue(collider.ColliderPtr->Filter.IsValid);

                // Character step input
                CharacterControllerStepInput stepInput = new CharacterControllerStepInput
                {
                    World            = PhysicsWorld,
                    DeltaTime        = DeltaTime,
                    Up               = math.up(),
                    Gravity          = ccComponentData.Gravity,
                    MaxIterations    = ccComponentData.MaxIterations,
                    Tau              = k_DefaultTau,
                    Damping          = k_DefaultDamping,
                    SkinWidth        = ccComponentData.SkinWidth,
                    ContactTolerance = ccComponentData.ContactTolerance,
                    MaxSlope         = ccComponentData.MaxSlope,
                    RigidBodyIndex   = PhysicsWorld.GetRigidBodyIndex(ccInternalData.Entity),
                    CurrentVelocity  = ccInternalData.LinearVelocity
                };

                // Character transform
                RigidTransform transform = new RigidTransform
                {
                    pos = position.Value,
                    rot = rotation.Value
                };

                // "Broad phase" (used both for checking support and actual character collide and integrate).
                MaxHitsCollector <DistanceHit> distanceHitsCollector = new MaxHitsCollector <DistanceHit>(
                    stepInput.RigidBodyIndex, ccComponentData.ContactTolerance, ref distanceHits);
                {
                    ColliderDistanceInput input = new ColliderDistanceInput()
                    {
                        MaxDistance = ccComponentData.ContactTolerance,
                        Transform   = transform,
                        Collider    = collider.ColliderPtr
                    };
                    PhysicsWorld.CalculateDistance(input, ref distanceHitsCollector);
                }

                // Check support
                CheckSupport(stepInput, transform, ccComponentData.MaxSlope, distanceHitsCollector,
                             ref constraints, out int numConstraints, out ccInternalData.SupportedState, out float3 surfaceNormal, out float3 surfaceVelocity);

                // User input
                float3 desiredVelocity = ccInternalData.LinearVelocity;
                HandleUserInput(ccComponentData, stepInput.Up, surfaceVelocity, ref ccInternalData, ref desiredVelocity);

                // Calculate actual velocity with respect to surface
                if (ccInternalData.SupportedState == CharacterSupportState.Supported)
                {
                    CalculateMovement(ccInternalData.CurrentRotationAngle, stepInput.Up, ccInternalData.IsJumping,
                                      ccInternalData.LinearVelocity, desiredVelocity, surfaceNormal, surfaceVelocity, out ccInternalData.LinearVelocity);
                }
                else
                {
                    ccInternalData.LinearVelocity = desiredVelocity;
                }

                // World collision + integrate
                CollideAndIntegrate(stepInput, ccComponentData.CharacterMass, ccComponentData.AffectsPhysicsBodies > 0,
                                    collider.ColliderPtr, ref castHits, ref constraints, numConstraints,
                                    ref transform, ref ccInternalData.LinearVelocity, ref DeferredImpulseWriter);

                // Write back and orientation integration
                position.Value = transform.pos;
                rotation.Value = quaternion.AxisAngle(up, ccInternalData.CurrentRotationAngle);

                // Write back to chunk data
                {
                    chunkCCInternalData[i]  = ccInternalData;
                    chunkTranslationData[i] = position;
                    chunkRotationData[i]    = rotation;
                }
            }

            DeferredImpulseWriter.EndForEachIndex();
        }