public static bool CastRayFromMouse(float3 cameraPos, float3 mouseWorldPos, float distance, out Unity.Physics.RaycastHit closestHit, CollisionFilter collisionFilter, CollisionWorld collisionWorld) { float3 origin = cameraPos; float3 direction = math.normalize(mouseWorldPos - origin); RaycastInput rayInput = new RaycastInput() { Start = origin, End = origin + (direction * distance), Filter = collisionFilter }; return(collisionWorld.CastRay(rayInput, out closestHit)); }
protected virtual void _InitializePhysicsWorld() { _isDisposed = false; if (m_worldType == WorldType.SoftBodyAndRigidBody && m_collisionType == CollisionConfType.DefaultDynamicsWorldCollisionConf) { BDebug.LogError(debugType, "For World Type = SoftBodyAndRigidBody collisionType must be collisionType=SoftBodyRigidBodyCollisionConf. Switching"); m_collisionType = CollisionConfType.SoftBodyRigidBodyCollisionConf; } if (m_collisionType == CollisionConfType.DefaultDynamicsWorldCollisionConf) { CollisionConf = new DefaultCollisionConfiguration(); } else if (m_collisionType == CollisionConfType.SoftBodyRigidBodyCollisionConf) { CollisionConf = new SoftBodyRigidBodyCollisionConfiguration(); } Dispatcher = new CollisionDispatcher(CollisionConf); if (m_broadphaseType == BroadphaseType.DynamicAABBBroadphase) { Broadphase = new DbvtBroadphase(); } else if (m_broadphaseType == BroadphaseType.Axis3SweepBroadphase) { Broadphase = new AxisSweep3(m_axis3SweepBroadphaseMin.ToBullet(), m_axis3SweepBroadphaseMax.ToBullet(), axis3SweepMaxProxies); } else if (m_broadphaseType == BroadphaseType.Axis3SweepBroadphase_32bit) { Broadphase = new AxisSweep3_32Bit(m_axis3SweepBroadphaseMin.ToBullet(), m_axis3SweepBroadphaseMax.ToBullet(), axis3SweepMaxProxies); } else { Broadphase = null; } if (m_worldType == WorldType.CollisionOnly) { m_world = new CollisionWorld(Dispatcher, Broadphase, CollisionConf); _ddWorld = null; } else if (m_worldType == WorldType.RigidBodyDynamics) { m_world = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf); _ddWorld = (DiscreteDynamicsWorld)m_world; } else if (m_worldType == WorldType.MultiBodyWorld) { m_world = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf); _ddWorld = (DiscreteDynamicsWorld)m_world; } else if (m_worldType == WorldType.SoftBodyAndRigidBody) { Solver = new SequentialImpulseConstraintSolver(); Solver.RandSeed = sequentialImpulseConstraintSolverRandomSeed; softBodyWorldInfo = new SoftBodyWorldInfo { AirDensity = 1.2f, WaterDensity = 0, WaterOffset = 0, WaterNormal = BulletSharp.Math.Vector3.Zero, Gravity = UnityEngine.Physics.gravity.ToBullet(), Dispatcher = Dispatcher, Broadphase = Broadphase }; softBodyWorldInfo.SparseSdf.Initialize(); m_world = new SoftRigidDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf); _ddWorld = (DiscreteDynamicsWorld)m_world; m_world.DispatchInfo.EnableSpu = true; softBodyWorldInfo.SparseSdf.Reset(); softBodyWorldInfo.AirDensity = 1.2f; softBodyWorldInfo.WaterDensity = 0; softBodyWorldInfo.WaterOffset = 0; softBodyWorldInfo.WaterNormal = BulletSharp.Math.Vector3.Zero; softBodyWorldInfo.Gravity = m_gravity.ToBullet(); } if (_ddWorld != null) { _ddWorld.Gravity = m_gravity.ToBullet(); } if (_doDebugDraw) { DebugDrawUnity db = new DebugDrawUnity(); db.DebugMode = _debugDrawMode; m_world.DebugDrawer = db; } }
public void UpdateAction(CollisionWorld collisionWorld, float deltaTimeStep) { UpdateVehicle(deltaTimeStep); }
// Update is called once per frame protected override void OnUpdate( ) { PhysicsWorld physicsWorld = buildPhysicsWorldSystem.PhysicsWorld; CollisionWorld collisionWorld = physicsWorld.CollisionWorld; // EntityCommandBuffer commandBuffer = m_EntityCommandBufferSystem.CreateCommandBuffer(); // Dependency = JobHandle.CombineDependencies(Dependency, m_EndFramePhysicsSystem.GetOutputDependency()); Dependency = JobHandle.CombineDependencies(Dependency, endFramePhysicsSystem.GetOutputDependency()); /* * Entities * .WithName ( "RaycastWithCustomCollectorJob" ) * .WithBurst () * .ForEach ( ( Entity entity, ref Translation position, ref Rotation rotation ) => * { * * * // var collector = new IgnoreTransparentClosestHitCollector ( collisionWorld ) ; * * // collisionWorld.CastRay ( raycastInput, ref collector ) ; * * //if ( collisionWorld.Bodies [hit.RigidBodyIndex].Collider, hit.ColliderKey ) * //{ * // return false; * //} * * // var collector = new IgnoreTransparentClosestHitCollector ( collisionWorld ) ; * * * var raycastLength = 200 ; * * // Perform the Raycast * var raycastInput = new RaycastInput * { * Start = position.Value, * End = position.Value + ( math.forward (rotation.Value) * raycastLength ), * Filter = CollisionFilter.Default * }; * * collisionWorld.CastRay(raycastInput, ref collector); * * var hit = collector.ClosestHit; * var hitDistance = raycastLength * hit.Fraction; * * Debug.LogWarning ( "Hit dist: " + hitDistance ) ; * * * }).Schedule () ; */ // Debug.Log ( "runs" ) ; // EntityCommandBuffer.ParallelWriter ecbp = becb.CreateCommandBuffer ().AsParallelWriter () ; // CollisionWorld collisionWorld = buildPhysicsWorldSystem.PhysicsWorld.CollisionWorld ; // Debug.DrawLine ( pointerRay.origin, pointerRay.direction * 200, Color.blue ) ; // RaycastInput raycastInput = new RaycastInput () { Start = pointerRay.origin, End = pointerRay.direction * 200, Filter = CollisionFilter.Default } ; // UnityEngine.Ray pointerRay = Camera.main.ScreenPointToRay ( Input.mousePosition ) ; Vector3 V3_point = Camera.main.ScreenToWorldPoint(Input.mousePosition); Debug.DrawLine(V3_point, V3_point - Vector3.up * 200, Color.blue); CollisionFilter collisionFilter = new CollisionFilter() { BelongsTo = default, // 1,
static private void InternalTickCallbackNative(IntPtr world, float timeStep) { CollisionWorld cw = _native2ManagedMap[world]; ((DynamicsWorld)cw)._callback((DynamicsWorld)cw, timeStep); }
protected bool RecoverFromPenetration(CollisionWorld collisionWorld) { Vector3 minAabb, maxAabb; m_convexShape.GetAabb(m_ghostObject.WorldTransform, out minAabb, out maxAabb); collisionWorld.Broadphase.SetAabbRef(m_ghostObject.BroadphaseHandle, ref minAabb, ref maxAabb, collisionWorld.Dispatcher); bool penetration = false; collisionWorld.Dispatcher.DispatchAllCollisionPairs(m_ghostObject.OverlappingPairCache, collisionWorld.DispatchInfo, collisionWorld.Dispatcher); m_currentPosition = m_ghostObject.WorldTransform.Origin; float maxPen = 0f; for (int i = 0; i < m_ghostObject.OverlappingPairCache.NumOverlappingPairs; i++) { m_manifoldArray.Clear(); BroadphasePair collisionPair = m_ghostObject.OverlappingPairCache.OverlappingPairArray[i]; CollisionObject obj0 = collisionPair.Proxy0.ClientObject as CollisionObject; CollisionObject obj1 = collisionPair.Proxy1.ClientObject as CollisionObject; if ((obj0 != null && !obj0.HasContactResponse) || (obj1 != null && !obj1.HasContactResponse)) { continue; } if (collisionPair.Algorithm != null) { collisionPair.Algorithm.GetAllContactManifolds(m_manifoldArray); } for (int j = 0; j < m_manifoldArray.Count; j++) { PersistentManifold manifold = m_manifoldArray[j]; float directionSign = manifold.Body0 == m_ghostObject ? -1f : 1f; for (int p = 0; p < manifold.NumContacts; p++) { ManifoldPoint pt = manifold.GetContactPoint(p); float dist = pt.Distance; if (dist < 0.0f) { if (dist < maxPen) { maxPen = dist; m_touchingNormal = pt.NormalWorldOnB * directionSign;//?? } m_currentPosition += pt.NormalWorldOnB * directionSign * dist * 0.2f; penetration = true; } else { //printf("touching %f\n", dist); } } //manifold.ClearManifold(); } } Matrix newTrans = m_ghostObject.WorldTransform; newTrans.Origin = m_currentPosition; m_ghostObject.WorldTransform = newTrans; // printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]); return(penetration); }
///btActionInterface interface public virtual void UpdateAction(CollisionWorld collisionWorld, float deltaTime) { PreStep(collisionWorld); PlayerStep(collisionWorld, deltaTime); }
public void StoreIslandActivationState(CollisionWorld world) { btSimulationIslandManager_storeIslandActivationState(Native, world.Native); }
public void UpdateActivationState(CollisionWorld colWorld, Dispatcher dispatcher) { btSimulationIslandManager_updateActivationState(Native, colWorld.Native, dispatcher.Native); }
public void BuildIslands(Dispatcher dispatcher, CollisionWorld colWorld) { btSimulationIslandManager_buildIslands(Native, dispatcher.Native, colWorld.Native); }
public void FindUnions(Dispatcher dispatcher, CollisionWorld colWorld) { btSimulationIslandManager_findUnions(Native, dispatcher.Native, colWorld.Native); }
public void BuildAndProcessIslands(Dispatcher dispatcher, CollisionWorld collisionWorld, IslandCallback callback) { btSimulationIslandManager_buildAndProcessIslands(Native, dispatcher.Native, collisionWorld.Native, callback.Native); }
public void cast(CollisionWorld cw) { #if USE_BT_CLOCK frame_timer.reset(); #endif //USE_BT_CLOCK #if BATCH_RAYCASTER if (gBatchRaycaster == null) { return; } gBatchRaycaster.clearRays(); for (int i = 0; i < NUMRAYS_IN_BAR; i++) { gBatchRaycaster.addRay(source[i], dest[i]); } gBatchRaycaster.performBatchRaycast(); for (int i = 0; i < gBatchRaycaster.getNumRays(); i++) { const SpuRaycastTaskWorkUnitOut& outResult = (*gBatchRaycaster)[i]; hit[i].setInterpolate3(source[i], dest[i], outResult.hitFraction); normal[i] = outResult.hitNormal; normal[i] = normal[i].Normalize(); } #else for (int i = 0; i < NUMRAYS_IN_BAR; i++) { using (ClosestRayResultCallback cb = BulletGlobals.ClosestRayResultCallbackPool.Get()) { cb.Initialize(source[i], dest[i]); cw.RayTest(ref source[i], ref dest[i], cb); if (cb.HasHit()) { hit[i] = cb.m_hitPointWorld; normal[i] = cb.m_hitNormalWorld; normal[i] = IndexedVector3.Normalize(normal[i]); } else { hit[i] = dest[i]; normal[i] = new IndexedVector3(1.0f, 0.0f, 0.0f); } } } #if USE_BT_CLOCK ms += frame_timer.getTimeMilliseconds(); #endif //USE_BT_CLOCK frame_counter++; if (frame_counter > 50) { min_ms = ms < min_ms ? ms : min_ms; max_ms = ms > max_ms ? ms : max_ms; sum_ms += ms; sum_ms_samples++; float mean_ms = (float)sum_ms / (float)sum_ms_samples; //printf("%d rays in %d ms %d %d %f\n", NUMRAYS_IN_BAR * frame_counter, ms, min_ms, max_ms, mean_ms); ms = 0; frame_counter = 0; } #endif }
/// <summary> /// Gets the final position of a character attempting to move from a starting /// location with a given movement. The goal of this is to move the character /// but have the character 'bounce' off of objects at angles that are /// are along the plane perpendicular to the normal of the surface hit. /// This will cancel motion through the object and instead deflect it /// into another direction giving the effect of sliding along objects /// while the character's momentum is absorbed into the wall. /// </summary> /// <param name="commandBuffer">Command buffer for adding push events to objects</param> /// <param name="jobIndex">Index of this job for command buffer use</param> /// <param name="collisionWorld">World for checking collisions and other colliable objects</param> /// <param name="start">Starting location</param> /// <param name="movement">Intended direction of movement</param> /// <param name="collider">Collider controlling the character</param> /// <param name="entityIndex">Index of this entity</param> /// <param name="rotation">Current character rotation</param> /// <param name="physicsMassAccessor">Accessor to physics mass components</param> /// <param name="anglePower">Power to raise decay of movement due to /// changes in angle between intended movement and angle of surface. /// Will be angleFactor= 1 / (1 + normAngle) where normAngle is a normalized value /// between 0-1 where 1 is max angle (90 deg) and 0 is min angle (0 degrees). /// AnglePower is the power to which the angle factor is raised /// for a sharper decline. Value of zero negates this property. /// <param name="maxBounces">Maximum number of bounces when moving. /// After this has been exceeded the bouncing will stop. By default /// this is one assuming that each move is fairly small this should approximate /// normal movement.</param> /// <param name="pushPower">Multiplier for power when pushing on an object. /// Larger values mean more pushing.</param> /// <param name="pushDecay">How much does the current push decay the remaining /// movement by. Values between [0, 1]. A zero would mean all energy is lost /// when pushing, 1 means no momentum is lost by pushing. A value of 0.5 /// would mean half of the energy is lost</param> /// <param name="epsilon">Epsilon parameter for pushing away from objects to avoid colliding /// with static objects. Should be some small float value that can be /// tuned for how much to push away while not allowing being shoved into objects.</param> /// <returns>The final location of the character.</returns> public static unsafe float3 ProjectValidMovement( EntityCommandBuffer.ParallelWriter commandBuffer, int jobIndex, CollisionWorld collisionWorld, float3 start, float3 movement, PhysicsCollider collider, int entityIndex, quaternion rotation, ComponentDataFromEntity <PhysicsMass> physicsMassGetter, float anglePower = 2, int maxBounces = 1, float pushPower = 25, float pushDecay = 0, float epsilon = 0.001f) { float3 from = start; // Starting location of movement float3 remaining = movement; // Remaining momentum int bounces = 0; // current number of bounces // Continue computing while there is momentum and bounces remaining while (math.length(remaining) > epsilon && bounces <= maxBounces) { // Get the target location given the momentum float3 target = from + remaining; // Do a cast of the collider to see if an object is hit during this // movement action var input = new ColliderCastInput() { Start = from, End = target, Collider = collider.ColliderPtr, Orientation = rotation }; SelfFilteringClosestHitCollector <ColliderCastHit> hitCollector = new SelfFilteringClosestHitCollector <ColliderCastHit>(entityIndex, 1.0f, collisionWorld); bool collisionOcurred = collisionWorld.CastCollider(input, ref hitCollector); if (!collisionOcurred && hitCollector.NumHits == 0) { // If there is no hit, target can be returned as final position return(target); } Unity.Physics.ColliderCastHit hit = hitCollector.ClosestHit; // Set the fraction of remaining movement (minus some small value) from = from + remaining * hit.Fraction; // Push slightly along normal to stop from getting caught in walls from = from + hit.SurfaceNormal * epsilon; // Apply some force to the object hit if it is moveable, Apply force on entity hit bool isKinematic = physicsMassGetter.HasComponent(hit.Entity) && IsKinematic(physicsMassGetter[hit.Entity]); if (hit.RigidBodyIndex < collisionWorld.NumDynamicBodies && !isKinematic) { commandBuffer.AddBuffer <PushForce>(jobIndex, hit.Entity); commandBuffer.AppendToBuffer(jobIndex, hit.Entity, new PushForce() { force = movement * pushPower, point = hit.Position }); // If pushing something, reduce remaining force significantly remaining *= pushDecay; } // Get angle between surface normal and remaining movement float angleBetween = math.length(math.dot(hit.SurfaceNormal, remaining)) / math.length(remaining); // Normalize angle between to be between 0 and 1 angleBetween = math.min(KCCUtils.MaxAngleShoveRadians, math.abs(angleBetween)); float normalizedAngle = angleBetween / KCCUtils.MaxAngleShoveRadians; // Create angle factor using 1 / (1 + normalizedAngle) float angleFactor = 1.0f / (1.0f + normalizedAngle); // If the character hit something // Reduce the momentum by the remaining movement that ocurred remaining *= (1 - hit.Fraction) * math.pow(angleFactor, anglePower); // Rotate the remaining remaining movement to be projected along the plane // of the surface hit (emulate pushing against the object) // A is our vector and B is normal of plane // A || B = B × (A×B / |B|) / |B| // From http://www.euclideanspace.com/maths/geometry/elements/plane/lineOnPlane/index.htm float3 planeNormal = hit.SurfaceNormal; float momentumLeft = math.length(remaining); remaining = math.cross(planeNormal, math.cross(remaining, planeNormal) / math.length(planeNormal)) / math.length(planeNormal); remaining = math.normalizesafe(remaining) * momentumLeft; // Track number of times the character has bounced bounces++; } return(from); }
protected bool RecoverFromPenetration(CollisionWorld collisionWorld) { Vector3 minAabb, maxAabb; m_convexShape.GetAabb(m_ghostObject.WorldTransform, out minAabb, out maxAabb); collisionWorld.Broadphase.SetAabbRef(m_ghostObject.BroadphaseHandle, ref minAabb, ref maxAabb, collisionWorld.Dispatcher); bool penetration = false; collisionWorld.Dispatcher.DispatchAllCollisionPairs(m_ghostObject.OverlappingPairCache, collisionWorld.DispatchInfo, collisionWorld.Dispatcher); m_currentPosition = m_ghostObject.WorldTransform.Origin; float maxPen = 0f; for (int i = 0; i < m_ghostObject.OverlappingPairCache.NumOverlappingPairs; i++) { m_manifoldArray.Clear(); BroadphasePair collisionPair = m_ghostObject.OverlappingPairCache.OverlappingPairArray[i]; CollisionObject obj0 = collisionPair.Proxy0.ClientObject as CollisionObject; CollisionObject obj1 = collisionPair.Proxy1.ClientObject as CollisionObject; if ((obj0 != null && !obj0.HasContactResponse) || (obj1 != null && !obj1.HasContactResponse)) { continue; } collisionPair.GetAllContactManifolds(m_manifoldArray); for (int j = 0; j < m_manifoldArray.Count; j++) { var manifoldId = AlignedManifoldArray.btAlignedManifoldArray_at(m_manifoldArray._native, j); var bodyId = PersistentManifold.btPersistentManifold_getBody0(manifoldId); var numContacts = PersistentManifold.btPersistentManifold_getNumContacts(manifoldId); float directionSign = bodyId == m_ghostObject._native ? -1f : 1f; for (int p = 0; p < numContacts; p++) { var manifoldPointId = PersistentManifold.btPersistentManifold_getContactPoint(manifoldId, p); float dist = ManifoldPoint.btManifoldPoint_getDistance(manifoldPointId); if (dist < 0.0f) { Vector3 normalWorldOnB; ManifoldPoint.btManifoldPoint_getNormalWorldOnB(manifoldPointId, out normalWorldOnB); if (dist < maxPen) { maxPen = dist; } var counterPenDir = normalWorldOnB * directionSign;; m_currentPosition += counterPenDir * dist; penetration = true; if (counterPenDir.Dot(Vector3.UnitY) > 0) { m_verticalVelocity = 0; } } } } } Matrix newTrans = m_ghostObject.WorldTransform; newTrans.Origin = m_currentPosition; m_ghostObject.WorldTransform = newTrans; return(penetration); }
public void Cast(CollisionWorld cw, float frameDelta) { #if BATCH_RAYCASTER if (!batchRaycaster) { return; } batchRaycaster.ClearRays(); foreach (var ray in _rays) { batchRaycaster.AddRay(ray.Source, ray.Destination); } batchRaycaster.PerformBatchRaycast(); for (int i = 0; i < batchRaycaster.NumRays; i++) { const SpuRaycastTaskWorkUnitOut& out = (*batchRaycaster)[i]; _rays[i].HitPoint.SetInterpolate3(_source[i], _destination[i], out.HitFraction); _rays[i].Normal = out.hitNormal; _rays[i].Normal.Normalize(); } #else foreach (var ray in _rays) { using (var cb = new ClosestRayResultCallback(ref ray.Source, ref ray.Destination)) { cw.RayTestRef(ref ray.Source, ref ray.Destination, cb); if (cb.HasHit) { ray.HitPoint = cb.HitPointWorld; ray.Normal = cb.HitNormalWorld; ray.Normal.Normalize(); } else { ray.HitPoint = ray.Destination; ray.Normal = new Vector3(1.0f, 0.0f, 0.0f); } } } _time += frameDelta; _frameCount++; if (_frameCount > 50) { if (_time < _timeMin) { _timeMin = _time; } if (_time > _timeMax) { _timeMax = _time; } _timeTotal += _time; _sampleCount++; float timeMean = _timeTotal / _sampleCount; PrintStats(); _time = 0; _frameCount = 0; } #endif }
protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove) { // printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]); // phase 2: forward and strafe Matrix start = Matrix.Identity, end = Matrix.Identity; m_targetPosition = m_currentPosition + walkMove; float fraction = 1.0f; float distance2 = (m_currentPosition - m_targetPosition).LengthSquared; // printf("distance2=%f\n",distance2); if (m_touchingContact) { float dot; Vector3.Dot(ref m_normalizedDirection, ref m_touchingNormal, out dot); if (dot > 0.0f) { //interferes with step movement //UpdateTargetPositionBasedOnCollision(ref m_touchingNormal, 0.0f, 1.0f); } } int maxIter = 10; while (fraction > 0.01f && maxIter-- > 0) { start.Origin = (m_currentPosition); end.Origin = (m_targetPosition); Vector3 sweepDirNegative = m_currentPosition - m_targetPosition; KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, sweepDirNegative, 0f); callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; float margin = m_convexShape.Margin; m_convexShape.Margin = margin + m_addedMargin; if (m_useGhostObjectSweepTest) { m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); } else { collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); } m_convexShape.Margin = margin; fraction -= callback.ClosestHitFraction; if (callback.HasHit) { // we moved only a fraction float hitDistance = (callback.HitPointWorld - m_currentPosition).Length; Vector3 hitNormalWorld = callback.HitNormalWorld; UpdateTargetPositionBasedOnCollision(ref hitNormalWorld, 0f, 1f); Vector3 currentDir = m_targetPosition - m_currentPosition; distance2 = currentDir.LengthSquared; if (distance2 > MathUtil.SIMD_EPSILON) { currentDir.Normalize(); /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */ float dot; Vector3.Dot(ref currentDir, ref m_normalizedDirection, out dot); if (dot <= 0.0f) { break; } } else { // printf("currentDir: don't normalize a zero vector\n"); break; } } else { // we moved whole way m_currentPosition = m_targetPosition; } // if (callback.m_closestHitFraction == 0.f) // break; } }
/// <summary> /// Processes a specific entity in the chunk. /// </summary> /// <param name="entity"></param> /// <param name="controller"></param> /// <param name="position"></param> /// <param name="rotation"></param> /// <param name="collider"></param> /// <param name="collisionWorld"></param> private void HandleChunk(ref Entity entity, ref CharacterControllerComponent controller, ref Translation position, ref Rotation rotation, ref PhysicsCollider collider, ref CollisionWorld collisionWorld) { float3 epsilon = new float3(0.0f, Epsilon, 0.0f) * -math.normalize(controller.Gravity); float3 currPos = position.Value + epsilon; quaternion currRot = rotation.Value; float3 gravityVelocity = controller.Gravity * DeltaTime; float3 verticalVelocity = (controller.VerticalVelocity + gravityVelocity); float3 horizontalVelocity = (controller.CurrentDirection * controller.CurrentMagnitude * controller.Speed * DeltaTime); if (controller.IsGrounded) { if (controller.Jump) { verticalVelocity = controller.JumpStrength * -math.normalize(controller.Gravity); } else { float3 gravityDir = math.normalize(gravityVelocity); float3 verticalDir = math.normalize(verticalVelocity); if (MathUtils.FloatEquals(math.dot(gravityDir, verticalDir), 1.0f)) { verticalVelocity = new float3(); } } } HandleHorizontalMovement(ref horizontalVelocity, ref entity, ref currPos, ref currRot, ref controller, ref collider, ref collisionWorld); currPos += horizontalVelocity; HandleVerticalMovement(ref verticalVelocity, ref entity, ref currPos, ref currRot, ref controller, ref collider, ref collisionWorld); currPos += verticalVelocity; CorrectForCollision(ref entity, ref currPos, ref currRot, ref controller, ref collider, ref collisionWorld); DetermineIfGrounded(entity, ref currPos, ref epsilon, ref controller, ref collider, ref collisionWorld); position.Value = currPos - epsilon; }
protected void StepDown(CollisionWorld collisionWorld, float dt) { Matrix start, end, end_double; bool runonce = false; // phase 3: down /*float additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0; * btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep); * float downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt; * btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; * m_targetPosition -= (step_drop + gravity_drop);*/ Vector3 orig_position = m_targetPosition; float downVelocity = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt; if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) { downVelocity = m_fallSpeed; } Vector3 step_drop = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, upAxisDirection[m_upAxis], m_maxSlopeCosine); callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; KinematicClosestNotMeConvexResultCallback callback2 = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, upAxisDirection[m_upAxis], m_maxSlopeCosine); callback2.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; callback2.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; while (true) { start = Matrix.Translation(m_currentPosition); end = Matrix.Translation(m_targetPosition); //set double test for 2x the step drop, to check for a large drop vs small drop end_double = Matrix.Translation(m_targetPosition - step_drop); if (m_useGhostObjectSweepTest) { m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); if (!callback.HasHit) { //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial) m_ghostObject.ConvexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld.DispatchInfo.AllowedCcdPenetration); } } else { // this works.... collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); if (!callback.HasHit) { //test a double fall height, to see if the character should interpolate it's fall (large) or not (small) m_ghostObject.ConvexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld.DispatchInfo.AllowedCcdPenetration); } } float downVelocity2 = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt; bool has_hit = false; if (bounce_fix == true) { has_hit = callback.HasHit || callback2.HasHit; } else { has_hit = callback2.HasHit; } if (downVelocity2 > 0.0f && downVelocity2 < m_stepHeight && has_hit == true && runonce == false && (m_wasOnGround || !m_wasJumping)) { //redo the velocity calculation when falling a small amount, for fast stairs motion //for larger falls, use the smoother/slower interpolated movement by not touching the target position m_targetPosition = orig_position; downVelocity = m_stepHeight; Vector3 step_drop2 = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop2; runonce = true; continue; //re-run previous tests } break; } if (callback.HasHit || runonce == true) { // we dropped a fraction of the height -> hit floor float fraction = (m_currentPosition.Y - callback.HitPointWorld.Y) / 2; //printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY()); if (bounce_fix == true) { if (full_drop == true) { Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition); } else { //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, fraction, out m_currentPosition); } } else { Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition); } full_drop = false; m_verticalVelocity = 0.0f; m_verticalOffset = 0.0f; m_wasJumping = false; } else { // we dropped the full height full_drop = true; if (bounce_fix == true) { downVelocity = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt; if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) { m_targetPosition += step_drop; //undo previous target change downVelocity = m_fallSpeed; step_drop = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; } } //printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY()); m_currentPosition = m_targetPosition; } }
/// <summary> /// Performs a collision correction at the specified position. /// </summary> /// <param name="entity"></param> /// <param name="currPos"></param> /// <param name="currRot"></param> /// <param name="controller"></param> /// <param name="collider"></param> /// <param name="collisionWorld"></param> private void CorrectForCollision(ref Entity entity, ref float3 currPos, ref quaternion currRot, ref CharacterControllerComponent controller, ref PhysicsCollider collider, ref CollisionWorld collisionWorld) { RigidTransform transform = new RigidTransform() { pos = currPos, rot = currRot }; // Use a subset sphere within our collider to test against. // We do not use the collider itself as some intersection (such as on ramps) is ok. var offset = -math.normalize(controller.Gravity) * 0.1f; var sampleCollider = new PhysicsCollider() { Value = SphereCollider.Create(new SphereGeometry() { Center = currPos + offset, Radius = 0.1f }) }; if (PhysicsUtils.ColliderDistance(out DistanceHit smallestHit, sampleCollider, 0.1f, transform, ref collisionWorld, entity, PhysicsCollisionFilters.DynamicWithPhysical, null, ColliderData, Allocator.Temp)) { if (smallestHit.Distance < 0.0f) { currPos += math.abs(smallestHit.Distance) * smallestHit.SurfaceNormal; } } }
///btActionInterface interface public virtual void UpdateAction(CollisionWorld collisionWorld, float step) { UpdateVehicle(step); }
/// <summary> /// Determines if the character is resting on a surface. /// </summary> /// <param name="entity"></param> /// <param name="currPos"></param> /// <param name="epsilon"></param> /// <param name="collider"></param> /// <param name="collisionWorld"></param> /// <returns></returns> private unsafe static void DetermineIfGrounded(Entity entity, ref float3 currPos, ref float3 epsilon, ref CharacterControllerComponent controller, ref PhysicsCollider collider, ref CollisionWorld collisionWorld) { var aabb = collider.ColliderPtr->CalculateAabb(); float mod = 0.15f; float3 samplePos = currPos + new float3(0.0f, aabb.Min.y, 0.0f); float3 gravity = math.normalize(controller.Gravity); float3 offset = (gravity * 0.1f); float3 posLeft = samplePos - new float3(aabb.Extents.x * mod, 0.0f, 0.0f); float3 posRight = samplePos + new float3(aabb.Extents.x * mod, 0.0f, 0.0f); float3 posForward = samplePos + new float3(0.0f, 0.0f, aabb.Extents.z * mod); float3 posBackward = samplePos - new float3(0.0f, 0.0f, aabb.Extents.z * mod); controller.IsGrounded = PhysicsUtils.Raycast(out RaycastHit centerHit, samplePos, samplePos + offset, ref collisionWorld, entity, PhysicsCollisionFilters.DynamicWithPhysical, Allocator.Temp) || PhysicsUtils.Raycast(out RaycastHit leftHit, posLeft, posLeft + offset, ref collisionWorld, entity, PhysicsCollisionFilters.DynamicWithPhysical, Allocator.Temp) || PhysicsUtils.Raycast(out RaycastHit rightHit, posRight, posRight + offset, ref collisionWorld, entity, PhysicsCollisionFilters.DynamicWithPhysical, Allocator.Temp) || PhysicsUtils.Raycast(out RaycastHit forwardHit, posForward, posForward + offset, ref collisionWorld, entity, PhysicsCollisionFilters.DynamicWithPhysical, Allocator.Temp) || PhysicsUtils.Raycast(out RaycastHit backwardHit, posBackward, posBackward + offset, ref collisionWorld, entity, PhysicsCollisionFilters.DynamicWithPhysical, Allocator.Temp); }
private void FixedUpdate() { CollisionWorld collisionWorld = GetWorld().World; collisionWorld.Dispatcher.DispatchAllCollisionPairs(m_ghostObject.OverlappingPairCache, collisionWorld.DispatchInfo, collisionWorld.Dispatcher); //m_currentPosition = m_ghostObject.WorldTransform.Origin; //float maxPen = 0f; objsCurrentlyInContactWith.Clear(); for (int i = 0; i < m_ghostObject.OverlappingPairCache.NumOverlappingPairs; i++) { manifoldArray.Clear(); BroadphasePair collisionPair = m_ghostObject.OverlappingPairCache.OverlappingPairArray[i]; CollisionObject obj0 = collisionPair.Proxy0.ClientObject as CollisionObject; CollisionObject obj1 = collisionPair.Proxy1.ClientObject as CollisionObject; if ((obj0 != null && !obj0.HasContactResponse) || (obj1 != null && !obj1.HasContactResponse)) { continue; } if (collisionPair.Algorithm != null) { collisionPair.Algorithm.GetAllContactManifolds(manifoldArray); } CollisionObject otherObj = null; if (manifoldArray.Count > 0) { PersistentManifold pm = manifoldArray[0]; if (pm.Body0 == collisionObject) { otherObj = pm.Body1; } else { otherObj = pm.Body0; } } else { continue; } objsCurrentlyInContactWith.Add(otherObj); if (!objsIWasInContactWithLastFrame.Contains(otherObj)) { BOnTriggerEnter(otherObj, manifoldArray); } else { BOnTriggerStay(otherObj, manifoldArray); } } objsIWasInContactWithLastFrame.ExceptWith(objsCurrentlyInContactWith); foreach (CollisionObject co in objsIWasInContactWithLastFrame) { BOnTriggerExit(co); } //swap the hashsets so objsIWasInContactWithLastFrame now contains the list of objs. HashSet <CollisionObject> temp = objsIWasInContactWithLastFrame; objsIWasInContactWithLastFrame = objsCurrentlyInContactWith; objsCurrentlyInContactWith = temp; }
protected override void OnUpdate() { CollisionFilter collisionFilter = new CollisionFilter() { BelongsTo = ~0u, CollidesWith = ~0u, // all 1s, so all layers, collide with everything GroupIndex = 0 }; EntityManager entityManager = World.EntityManager; CollisionWorld collisionWorld = physicsWorld.PhysicsWorld.CollisionWorld; Dependency = Entities.WithAll <AnimalTag>().ForEach((ref AnimalMovementData mvmtData, in Translation translation) => { float3 start = new float3(translation.Value.x, translation.Value.y, translation.Value.z + 0.2f); #region front RaycastInput inputFront = new RaycastInput() { Start = start, End = new float3(translation.Value.x, translation.Value.y, translation.Value.z + (2.5f * mvmtData.movementSpeed)), Filter = collisionFilter }; RaycastHit hitFront = new RaycastHit(); bool hasHitFront = collisionWorld.CastRay(inputFront, out hitFront); #endregion #region front right RaycastInput inputFR = new RaycastInput() { Start = start, End = new float3(translation.Value.x + (2.5f * mvmtData.movementSpeed), translation.Value.y, translation.Value.z + (2.5f * mvmtData.movementSpeed)), Filter = collisionFilter }; RaycastHit hitFR = new RaycastHit(); bool hasHitFR = collisionWorld.CastRay(inputFR, out hitFR); #endregion #region front left RaycastInput inputFL = new RaycastInput() { Start = start, End = new float3(translation.Value.x - (2.5f * mvmtData.movementSpeed), translation.Value.y, translation.Value.z + (2.5f * mvmtData.movementSpeed)), Filter = collisionFilter }; RaycastHit hitFL = new RaycastHit(); bool hasHitFL = collisionWorld.CastRay(inputFR, out hitFL); #endregion if (hasHitFront) { float3 normal = hitFront.SurfaceNormal; //AnimalMovementData mvmtDataHitEntity = entityManager.GetComponentData<AnimalMovementData>(hitEntity); //float maxSpeed = math.max(mvmtData.movementSpeed, mvmtDataHitEntity.movementSpeed); float dotProduct = math.dot(math.normalize(mvmtData.targetDirection - translation.Value), math.normalize(hitFront.SurfaceNormal)); float angle = 0.262f; angle += 1f - math.abs(dotProduct); //angle += (1f - (maxSpeed * 0.1f)); angle = math.clamp(angle, 0f, StaticValues.AVOIDANCE_MIN_ANGLE); quaternion newRotation = quaternion.RotateY(angle); float3 newDirection = math.rotate(newRotation, mvmtData.direction); newDirection = math.normalizesafe(newDirection); // Set EntityA's target direction mvmtData.targetDirection = newDirection; } else if (hasHitFR) { float3 normal = hitFR.SurfaceNormal; //AnimalMovementData mvmtDataHitEntity = entityManager.GetComponentData<AnimalMovementData>(hitEntity); //float maxSpeed = math.max(mvmtData.movementSpeed, mvmtDataHitEntity.movementSpeed); float dotProduct = math.dot(math.normalize(mvmtData.targetDirection - translation.Value), math.normalize(hitFR.SurfaceNormal)); float angle = 0.262f; angle += 1f - math.abs(dotProduct); //angle += (1f - (maxSpeed * 0.1f)); angle = math.clamp(angle, 0f, StaticValues.AVOIDANCE_MIN_ANGLE); quaternion newRotation = quaternion.RotateY(angle); float3 newDirection = math.rotate(newRotation, mvmtData.direction); newDirection = math.normalizesafe(newDirection); // Set EntityA's target direction mvmtData.targetDirection = newDirection; } else if (hasHitFL) { float3 normal = hitFR.SurfaceNormal; //AnimalMovementData mvmtDataHitEntity = entityManager.GetComponentData<AnimalMovementData>(hitEntity); //float maxSpeed = math.max(mvmtData.movementSpeed, mvmtDataHitEntity.movementSpeed); float dotProduct = math.dot(math.normalize(mvmtData.targetDirection - translation.Value), math.normalize(hitFR.SurfaceNormal)); float angle = 0.262f; angle += 1f - math.abs(dotProduct); //angle += (1f - (maxSpeed * 0.1f)); angle = math.clamp(angle, 0f, StaticValues.AVOIDANCE_MIN_ANGLE); quaternion newRotation = quaternion.RotateY(angle); float3 newDirection = math.rotate(newRotation, mvmtData.direction); newDirection = math.normalizesafe(newDirection); // Set EntityA's target direction mvmtData.targetDirection = newDirection; } }).Schedule(Dependency); Dependency.Complete(); }
//Does not set any local variables. Is safe to use to create duplicate physics worlds for independant simulation. public bool CreatePhysicsWorld ( out CollisionWorld world, out CollisionConfiguration collisionConfig, out CollisionDispatcher dispatcher, out BroadphaseInterface broadphase, out ConstraintSolver solver, out SoftBodyWorldInfo softBodyWorldInfo ) { bool success = true; if (m_worldType == WorldType.SoftBodyAndRigidBody && m_collisionType == CollisionConfType.DefaultDynamicsWorldCollisionConf) { Debug.LogError("For World Type = SoftBodyAndRigidBody collisionType must be collisionType=SoftBodyRigidBodyCollisionConf. Switching"); m_collisionType = CollisionConfType.SoftBodyRigidBodyCollisionConf; success = false; } collisionConfig = null; switch (m_collisionType) { default: case CollisionConfType.DefaultDynamicsWorldCollisionConf: collisionConfig = new DefaultCollisionConfiguration(); break; case CollisionConfType.SoftBodyRigidBodyCollisionConf: collisionConfig = new SoftBodyRigidBodyCollisionConfiguration(); break; } dispatcher = new CollisionDispatcher(collisionConfig); switch (m_broadphaseType) { default: case BroadphaseType.DynamicAABBBroadphase: broadphase = new DbvtBroadphase(); break; case BroadphaseType.Axis3SweepBroadphase: broadphase = new AxisSweep3(m_axis3SweepBroadphaseMin.ToBullet(), m_axis3SweepBroadphaseMax.ToBullet(), axis3SweepMaxProxies); break; case BroadphaseType.Axis3SweepBroadphase_32bit: broadphase = new AxisSweep3_32Bit(m_axis3SweepBroadphaseMin.ToBullet(), m_axis3SweepBroadphaseMax.ToBullet(), axis3SweepMaxProxies); break; case BroadphaseType.SimpleBroadphase: broadphase = null; success = false; break; } world = null; softBodyWorldInfo = null; solver = null; switch (m_worldType) { case WorldType.CollisionOnly: world = new CollisionWorld(dispatcher, broadphase, collisionConfig); break; default: case WorldType.RigidBodyDynamics: world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, collisionConfig); break; case WorldType.MultiBodyWorld: MultiBodyConstraintSolver mbConstraintSolver = new MultiBodyConstraintSolver(); constraintSolver = mbConstraintSolver; world = new MultiBodyDynamicsWorld(dispatcher, broadphase, mbConstraintSolver, collisionConfig); break; case WorldType.SoftBodyAndRigidBody: SequentialImpulseConstraintSolver siConstraintSolver = new SequentialImpulseConstraintSolver(); constraintSolver = siConstraintSolver; siConstraintSolver.RandSeed = sequentialImpulseConstraintSolverRandomSeed; world = new SoftRigidDynamicsWorld(Dispatcher, Broadphase, siConstraintSolver, CollisionConf); world.DispatchInfo.EnableSpu = true; SoftRigidDynamicsWorld _sworld = (SoftRigidDynamicsWorld)world; _sworld.WorldInfo.SparseSdf.Initialize(); _sworld.WorldInfo.SparseSdf.Reset(); _sworld.WorldInfo.AirDensity = 1.2f; _sworld.WorldInfo.WaterDensity = 0; _sworld.WorldInfo.WaterOffset = 0; _sworld.WorldInfo.WaterNormal = BulletSharp.Math.Vector3.Zero; _sworld.WorldInfo.Gravity = m_gravity.ToBullet(); break; } if (world is DiscreteDynamicsWorld) { ((DiscreteDynamicsWorld)world).Gravity = m_gravity.ToBullet(); } if (_doDebugDraw) { DebugDrawUnity db = new DebugDrawUnity(); db.DebugMode = _debugDrawMode; world.DebugDrawer = db; } return(success); }
protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove) { Matrix start = Matrix.Identity, end = Matrix.Identity; m_targetPosition = m_currentPosition + walkMove; float fraction = 1.0f; float distance2 = (m_currentPosition - m_targetPosition).LengthSquared; int maxIter = 10; while (fraction > 0.01f && maxIter-- > 0) { start.Origin = (m_currentPosition); end.Origin = (m_targetPosition); Vector3 sweepDirNegative = m_currentPosition - m_targetPosition; _callback.MinSlopeDot = 0; _callback.Up = sweepDirNegative; _callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; _callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; _callback.ClosestHitFraction = 1; float margin = m_convexShape.Margin; m_convexShape.Margin = margin + m_addedMargin; if (m_useGhostObjectSweepTest) { m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); } else { collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); } m_convexShape.Margin = margin; fraction -= _callback.ClosestHitFraction; if (_callback.HasHit) { Vector3 hitNormalWorld = _callback.HitNormalWorld; UpdateTargetPositionBasedOnCollision(ref hitNormalWorld, 0f, 1f); Vector3 currentDir = m_targetPosition - m_currentPosition; distance2 = currentDir.LengthSquared; if (distance2 > MathUtil.SIMD_EPSILON) { currentDir.Normalize(); /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */ Vector3.Dot(ref currentDir, ref m_normalizedDirection, out var dot); if (dot <= 0.0f) { break; } } else { break; } } else { m_currentPosition = m_targetPosition; } } }
protected void Dispose(bool disposing) { if (debugType >= BDebug.DebugType.Debug) { Debug.Log("BDynamicsWorld Disposing physics."); } if (m_world != null) { //remove/dispose constraints int i; if (_ddWorld != null) { if (debugType >= BDebug.DebugType.Debug) { Debug.LogFormat("Removing Constraints {0}", _ddWorld.NumConstraints); } for (i = _ddWorld.NumConstraints - 1; i >= 0; i--) { TypedConstraint constraint = _ddWorld.GetConstraint(i); _ddWorld.RemoveConstraint(constraint); if (constraint.Userobject is BTypedConstraint) { ((BTypedConstraint)constraint.Userobject).m_isInWorld = false; } if (debugType >= BDebug.DebugType.Debug) { Debug.LogFormat("Removed Constaint {0}", constraint.Userobject); } constraint.Dispose(); } } if (debugType >= BDebug.DebugType.Debug) { Debug.LogFormat("Removing Collision Objects {0}", _ddWorld.NumCollisionObjects); } //remove the rigidbodies from the dynamics world and delete them for (i = m_world.NumCollisionObjects - 1; i >= 0; i--) { CollisionObject obj = m_world.CollisionObjectArray[i]; RigidBody body = obj as RigidBody; if (body != null && body.MotionState != null) { Debug.Assert(body.NumConstraintRefs == 0, "Rigid body still had constraints"); body.MotionState.Dispose(); } m_world.RemoveCollisionObject(obj); if (obj.UserObject is BCollisionObject) { ((BCollisionObject)obj.UserObject).isInWorld = false; } if (debugType >= BDebug.DebugType.Debug) { Debug.LogFormat("Removed CollisionObject {0}", obj.UserObject); } obj.Dispose(); } if (m_world.DebugDrawer != null) { if (m_world.DebugDrawer is IDisposable) { IDisposable dis = (IDisposable)m_world.DebugDrawer; dis.Dispose(); } } m_world.Dispose(); Broadphase.Dispose(); Dispatcher.Dispose(); CollisionConf.Dispose(); _ddWorld = null; m_world = null; } if (Broadphase != null) { Broadphase.Dispose(); Broadphase = null; } if (Dispatcher != null) { Dispatcher.Dispose(); Dispatcher = null; } if (CollisionConf != null) { CollisionConf.Dispose(); CollisionConf = null; } if (Solver != null) { Solver.Dispose(); Solver = null; } if (softBodyWorldInfo != null) { softBodyWorldInfo.Dispose(); softBodyWorldInfo = null; } _isDisposed = true; singleton = null; }
protected void StepDown(CollisionWorld collisionWorld, float dt) { Matrix start, end, end_double; bool runonce = false; Vector3 orig_position = m_targetPosition; float downVelocity = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt; if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) { downVelocity = m_fallSpeed; } Vector3 step_drop = Vector3.UnitY * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; _callback.MinSlopeDot = m_maxSlopeCosine; _callback.Up = Vector3.UnitY; _callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; _callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; bool hasFirstHit; float hitFraction; Vector3 pointWorld; while (true) { start = Matrix.Translation(m_currentPosition); end = Matrix.Translation(m_targetPosition); //set double test for 2x the step drop, to check for a large drop vs small drop end_double = Matrix.Translation(m_targetPosition - step_drop); bool has_hit_first = false; bool has_hit_second = false; if (m_useGhostObjectSweepTest) { _callback.ClosestHitFraction = 1; m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); pointWorld = _callback.HitPointWorld; has_hit_first = _callback.HasHit; hitFraction = _callback.ClosestHitFraction; if (!has_hit_first) { _callback.ClosestHitFraction = 1; //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial) m_ghostObject.ConvexSweepTest(m_convexShape, start, end_double, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); has_hit_second = _callback.HasHit; } } else { _callback.ClosestHitFraction = 1; // this works.... collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); pointWorld = _callback.HitPointWorld; has_hit_first = _callback.HasHit; hitFraction = _callback.ClosestHitFraction; if (!has_hit_first) { _callback.ClosestHitFraction = 1; //test a double fall height, to see if the character should interpolate it's fall (large) or not (small) m_ghostObject.ConvexSweepTest(m_convexShape, start, end_double, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); has_hit_second = _callback.HasHit; } } hasFirstHit = has_hit_first; float downVelocity2 = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt; bool has_hit; if (bounce_fix == true) { has_hit = has_hit_first || has_hit_second; } else { has_hit = has_hit_second; } if (downVelocity2 > 0.0f && downVelocity2 < m_stepHeight && has_hit == true && runonce == false && (m_wasOnGround || !m_wasJumping)) { //redo the velocity calculation when falling a small amount, for fast stairs motion //for larger falls, use the smoother/slower interpolated movement by not touching the target position m_targetPosition = orig_position; downVelocity = m_stepHeight; Vector3 step_drop2 = Vector3.UnitY * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop2; runonce = true; continue; //re-run previous tests } break; } if (hasFirstHit || runonce) { // we dropped a fraction of the height -> hit floor float fraction = (m_currentPosition.Y - pointWorld.Y) / 2; if (bounce_fix) { if (full_drop) { Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, hitFraction, out m_currentPosition); } else { //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, fraction, out m_currentPosition); } } else { Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, hitFraction, out m_currentPosition); } full_drop = false; m_verticalVelocity = 0.0f; m_verticalOffset = 0.0f; m_wasJumping = false; } else { // we dropped the full height full_drop = true; if (bounce_fix == true) { downVelocity = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt; if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) { m_targetPosition += step_drop; //undo previous target change downVelocity = m_fallSpeed; step_drop = Vector3.UnitY * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; } } m_currentPosition = m_targetPosition; } }
/// <summary> /// Creates a self filtering object collision detection /// </summary> /// <param name="ignore">index of entity to ignore hitting</param> /// <param name="allow">index of entity to allow to hit, leave empty to allow everything (except the ignroe)</param> /// <param name="maxFraction">Maximum fraction that an object can be encountered /// as a portion of the current raycast draw</param> /// <param name="collisionWorld">World of all colliable objects</param> public FilteringClosestHitCollector(int ignore, int allow, float maxFraction, CollisionWorld collisionWorld) { this.MaxFraction = maxFraction; this.ClosestHit = default; this.NumHits = 0; this.ignore = ignore; this.allow = allow; this.collisionWorld = collisionWorld; }
protected unsafe override void OnUpdate() { var _ecb = m_EntityCommandBufferSystem.CreateCommandBuffer(); var _ecbo = m_EntityCommandBufferSystemo.CreateCommandBuffer(); var _ecbEndSimilation = m_endSimulationEntityCommandBufferSystem.CreateCommandBuffer(); var _ecbBeginPresentation = m_beginPresentationEntityCommandBufferSystem.CreateCommandBuffer(); var _physicsWorldSystem = World.DefaultGameObjectInjectionWorld.GetExistingSystem <Unity.Physics.Systems.BuildPhysicsWorld>(); CollisionWorld _collisionWorld = _physicsWorldSystem.PhysicsWorld.CollisionWorld; var _physicsWorld = _physicsWorldSystem.PhysicsWorld; float3 _startPosition = ECS_RTSControls.instance._startScreenPosition; float3 _endPosition = ECS_RTSControls.instance._endScreenPosition; float3 _lowerLeftPosition = ECS_RTSControls.instance._lowerLeftScreenPosition; float3 _upperRightPosition = ECS_RTSControls.instance._upperRightScreenPosition; Dependency = JobHandle.CombineDependencies(Dependency, m_EndFramePhysicsSystem.GetOutputDependency()); if (_inputReady) { if (Input_Mouse_0_Up == true && Input_Mouse_0_Up != EntityManager.HasComponent <Input_Mouse_0_Up>(_inputEntity)) { Input_Mouse_0_GetDown = true; Input_Mouse_0_Up = false; } if (Input_Mouse_0_Up == false && (Input_Mouse_0_Up != EntityManager.HasComponent <Input_Mouse_0_Up>(_inputEntity))) { Input_Mouse_0_GetUp = true; Input_Mouse_0_Up = true; } if (Input_Mouse_1_Up == true && (Input_Mouse_1_Up != EntityManager.HasComponent <Input_Mouse_1_Up>(_inputEntity))) { Input_Mouse_1_GetDown = true; Input_Mouse_1_Up = false; } if (Input_Mouse_1_Up == false && (Input_Mouse_1_Up != EntityManager.HasComponent <Input_Mouse_1_Up>(_inputEntity))) { Input_Mouse_1_GetUp = true; Input_Mouse_1_Up = true; } } else { if (_queryInput.CalculateEntityCount() > 0) { _inputEntity = _queryInput.GetSingletonEntity(); _inputReady = true; Input_Mouse_0_Up = EntityManager.HasComponent <Input_Mouse_0_Up>(_inputEntity); Input_Mouse_1_Up = EntityManager.HasComponent <Input_Mouse_1_Up>(_inputEntity); } } if (Time.ElapsedTime > 4) { /*this.Dependency = Entities.WithoutBurst().ForEach((in Translation _translation, in UnitSelected _unitSelected, in Selectable _selectable, in GrisTag _grisTag, in Entity _entity) => * { * var e = _ecbEndSimilation.Instantiate(PrefabConvert._DalleVerte); * _ecbEndSimilation.SetComponent<Translation>(e, _translation); * _ecbEndSimilation.DestroyEntity(_entity); * Debug.Log("Unity3d : 'va te faire foutre'"); * * }).Schedule(this.Dependency);*/ } if (Input_Mouse_0_GetUp == true) { bool selectOnlyOneEntity = false; float selectionAreaMinSize = 10f; float selectionAreaSize = math.distance(_lowerLeftPosition, _upperRightPosition); if (selectionAreaSize < selectionAreaMinSize) { // Selection area too small _lowerLeftPosition += new float3(-1, -1, 0) * (selectionAreaMinSize - selectionAreaSize) * .5f; _upperRightPosition += new float3(+1, +1, 0) * (selectionAreaMinSize - selectionAreaSize) * .5f; selectOnlyOneEntity = true; } // Deselect all selected Entities this.Dependency = Entities.WithAll <UnitSelected>().ForEach((Entity entity) => { _ecb.RemoveComponent <UnitSelected>(entity); }).Schedule(this.Dependency); Unity.Physics.RaycastHit _hit1 = new Unity.Physics.RaycastHit(); Unity.Physics.RaycastHit _hit2 = new Unity.Physics.RaycastHit(); Unity.Physics.RaycastHit _hit3 = new Unity.Physics.RaycastHit(); Unity.Physics.RaycastHit _hit4 = new Unity.Physics.RaycastHit(); Unity.Physics.RaycastHit _hit5 = new Unity.Physics.RaycastHit(); var _ray1 = ECS_RTSControls.instance._ray1; var _ray2 = ECS_RTSControls.instance._ray2; var _ray3 = ECS_RTSControls.instance._ray3; var _ray4 = ECS_RTSControls.instance._ray4; var _ray5 = ECS_RTSControls.instance._ray5; /*RaycastInput _inputo = new RaycastInput() * { * Start = _ray1.origin, * End = _ray1.origin + _ray1.direction * 1000, * Filter = new CollisionFilter() * { * BelongsTo = (uint)10 << 10, * CollidesWith = (uint)7 << 8, * GroupIndex = 8 * } * };*/ GetRaycastHit(_collisionWorld, _ray1, _ray2, _ray3, _ray4, _ray5, out _hit1, out _hit2, out _hit3, out _hit4, out _hit5); var _filter = new CollisionFilter() { BelongsTo = ~1u, CollidesWith = ~1u, // all 1s, so all layers, collide with everything GroupIndex = 0 }; NativeList <ColliderCastHit> _allHits = new NativeList <ColliderCastHit>(Allocator.TempJob); _polygonCollider = Unity.Physics.PolygonCollider.CreateQuad(_hit1.Position, _hit2.Position, _hit3.Position, _hit4.Position, _filter); /*SphereGeometry sphereGeometry = new SphereGeometry() { Center = float3.zero, Radius = 3 }; * _sphereCollider = Unity.Physics.SphereCollider.Create(sphereGeometry, _filter);*/ /* * Unity.Physics.ColliderCastInput colliderCastInput = new ColliderCastInput() * { * Collider = (Unity.Physics.Collider*)_sphereCollider.GetUnsafePtr(), * Orientation = ECS_RTSControls.instance._camera.transform.rotation, * Start = ECS_RTSControls.instance._camera.transform.position, * End = _ray1.origin + _ray1.direction * 1000 * };*/ var _moyPoly = new float3((_hit1.Position.x + _hit2.Position.x + _hit3.Position.x + _hit4.Position.x) / 4, (_hit1.Position.y + _hit2.Position.y + _hit3.Position.y + _hit4.Position.y) / 4, (_hit1.Position.z + _hit2.Position.z + _hit3.Position.z + _hit4.Position.z) / 4); Debug.Log(_moyPoly + "_moyPoly"); //var _pouitCollider = BlobAssetReference<Unity.Physics.Collider>.Create(_polygonCollider.GetUnsafePtr(), 1); /* this.Dependency = Job.WithoutBurst().WithCode(() => * { * var e = _ecb.Instantiate(PrefabConvert._CubeRouge); * _ecb.SetComponent<Translation>(e, new Translation { Value = _hit1.Position + new float3(0, 5, 0) }); * e = _ecb.Instantiate(PrefabConvert._CubeRouge); * _ecb.SetComponent<Translation>(e, new Translation { Value = _hit2.Position + new float3(0, 5, 0) }); * e = _ecb.Instantiate(PrefabConvert._CubeRouge); * _ecb.SetComponent<Translation>(e, new Translation { Value = _hit3.Position + new float3(0, 5, 0) }); * e = _ecb.Instantiate(PrefabConvert._CubeRouge); * _ecb.SetComponent<Translation>(e, new Translation { Value = _hit4.Position + new float3(0, 5, 0) }); * }).Schedule(this.Dependency); * Debug.Log(_dalleSelectedQuery.CalculateEntityCount() + "_dalleSelectedQueryCount"); * Debug.Log(_dalleQuery.CalculateEntityCount() + "_dalleQueryCount"); * * /*Mesh _mesh = new Mesh { } * * RenderMesh huiaef = new RenderMesh { mesh = } * * this.Dependency.Complete();*/ ColliderCastJob _colliderCastJob = new ColliderCastJob(); _colliderCastJob.CollectAllHits = true; _colliderCastJob.Collider = (Unity.Physics.Collider *)_polygonCollider.GetUnsafePtr(); _colliderCastJob.ColliderCastHits = _allHits; _colliderCastJob.End = new float3(0, -2, 0); // _moyPoly; _colliderCastJob.Orientation = quaternion.identity; //ECS_RTSControls.instance._camera.transform.rotation; _colliderCastJob.Start = /*_moyPoly +*/ new float3(0, 5, 0); _colliderCastJob.World = _physicsWorld; Dependency = _colliderCastJob.ScheduleSingle(_query, this.Dependency); //World.GetOrCreateSystem<StepPhysicsWorld>().FinalSimulationJobHandle.Complete(); this.Dependency.Complete(); /*Debug.Log(_query.CalculateEntityCount() + "_queryMovestats"); * Debug.Log(_allHits.Length + "_allHits.Length");*/ this.Dependency = Job.WithoutBurst().WithDisposeOnCompletion(_allHits).WithCode(() => { for (int i = 0; i < _allHits.Length; i++) { _ecbEndSimilation.AddComponent(_allHits[i].Entity, new UnitSelected()); } Debug.Log(_allHits.Length + "entityHits"); }).Schedule(this.Dependency); this.Dependency.Complete(); } if (Input_Mouse_1_Up == false) { if (Time.ElapsedTime > 2) { Debug.Log("yoloZoulou"); this.Dependency = Entities.WithoutBurst().WithAll <PlaneLayer>().WithAll <Player1>().ForEach((Entity entity) => { _ecb.DestroyEntity(entity);//.RemoveComponent<UnitSelected>(entity); }).Schedule(this.Dependency); } //Debug.Log(Input.GetMouseButtonDown(1)); // Right mouse button down float3 targetPosition = UtilsClass.GetMouseWorldPosition(); List <float3> movePositionList = GetPositionListAround(targetPosition, new float[] { 10f, 20f, 30f }, new int[] { 5, 10, 20 }); int positionIndex = 0; Entities.WithAll <UnitSelected>().ForEach((Entity entity, ref MoveStats _moveStats) => { /*moveTo.position = movePositionList[positionIndex]; * positionIndex = (positionIndex + 1) % movePositionList.Count; * moveTo.move = true;*/ }).Schedule(); } //m_EntityCommandBufferSystem.AddJobHandleForProducer(this.Dependency); World.GetOrCreateSystem <StepPhysicsWorld>().FinalSimulationJobHandle.Complete(); this.Dependency.Complete(); m_EntityCommandBufferSystemo.AddJobHandleForProducer(this.Dependency); m_endSimulationEntityCommandBufferSystem.AddJobHandleForProducer(this.Dependency); m_EntityCommandBufferSystemo.AddJobHandleForProducer(this.Dependency); if (_polygonCollider.IsCreated) { _polygonCollider.Dispose(); } if (_sphereCollider.IsCreated) { _sphereCollider.Dispose(); } Input_Mouse_0_GetUp = false; Input_Mouse_0_GetDown = false; }