import { Octree } from "./octree" ; import { Collision } from "./collision" ; import { CollisionShape , CombinedCollisionShape , SingletonCollisionShape } from "./collision_shape" ; import { CollisionDetection } from "./collision_detection" ; import { RigidBody , RigidBodyType } from "./rigid_body" ; import { CollisionResponse } from "./collision_response" ; import { Vector3 } from "../math/vector3" ; import { Box3 } from "../math/box3" ; import { Plane } from "../math/plane" ; const MAX_SUBSTEPS = 10 ; let v1 = new Vector3 ( ) ; let v2 = new Vector3 ( ) ; let v3 = new Vector3 ( ) ; let p1 = new Plane ( ) ; let rayCastAabb = new Box3 ( ) ; let singletonShape = new SingletonCollisionShape ( ) ; let combinedCollisionShape = new CombinedCollisionShape ( null , null ) ; let utilBody = new RigidBody ( ) ; utilBody . addCollisionShape ( singletonShape ) ; utilBody . addCollisionShape ( combinedCollisionShape ) ; export interface RayCastHit { point : Vector3 ; normal : Vector3 ; lambda : number ; shape : CollisionShape ; } /** Represents a physics simulation world. */ export class World { bodies : RigidBody [ ] = [ ] ; gravity = new Vector3 ( ) ; octree = new Octree ( ) ; /** List of shapes that are currently in contact according to the CCD (Continuous Collision Detection) pass. */ inContactCcd = new Set < CollisionShape > ( ) ; newInContactCcd = new Set < CollisionShape > ( ) ; /** List of shapes that are currently in contact. */ inContact = new Set < CollisionShape > ( ) ; newInContact = new Set < CollisionShape > ( ) ; /** This cache can speed up broadphase lookups by reusing results. */ cachedBroadphaseResults = new Map < CollisionShape , CollisionShape [ ] > ( ) ; add ( body ) { if ( body . world ) { throw new Error ( "RigidBody already belongs to a world." ) ; } this . bodies . push ( body ) ; body . world = this ; body . syncShapes ( ) ; } /** Steps the physics world by `dt` seconds. */ step ( dt ) { this . substep ( dt , 0 , 0 ) ; } substep ( dt , startT , depth ) { if ( dt < 0.00001 || depth >= MAX_SUBSTEPS ) return ; let dynamicBodies = [ ] ; let dynamicShapes = [ ] ; for ( let i = 0 ; i < this . bodies . length ; i ++ ) { let body = this . bodies [ i ] ; if ( ! body . enabled ) continue ; body . storePrevious ( ) ; body . onBeforeIntegrate ( dt ) ; body . integrate ( dt ) ; body . onAfterIntegrate ( dt ) ; body . collisions . length = 0 ; if ( body . type === RigidBodyType . Dynamic ) { dynamicBodies . push ( body ) ; dynamicShapes . push ( ... body . shapes ) ; } } this . cachedBroadphaseResults . clear ( ) ; let ccdCollisions = this . computeCollisions ( dynamicShapes , true ) ; let t = 1 ; for ( let i = 0 ; i < ccdCollisions . length ; i ++ ) { let collision = ccdCollisions [ i ] ; if ( ! this . inContactCcd . has ( collision . s2 ) ) { t = Math . min ( collision . timeOfImpact , t ) ; } } let cumT = startT + t * ( 1 - startT ) ; for ( let i = 0 ; i < this . bodies . length ; i ++ ) { let body = this . bodies [ i ] ; if ( ! body . enabled ) continue ; body . revert ( t ) ; body . collisions . length = 0 ; if ( body . type !== RigidBodyType . Dynamic ) continue ; let externalForce = v1 . set ( 0 , 0 , 0 ) ; externalForce . add ( this . gravity ) ; body . linearVelocity . addScaledVector ( externalForce , dt * t ) ; } let collisions = this . computeCollisions ( dynamicShapes , false ) ; let collidingBodies = [ ] ; for ( let i = 0 ; i < collisions . length ; i ++ ) { let collision = collisions [ i ] ; if ( ! collidingBodies . includes ( collision . s1 . body ) ) collidingBodies . push ( collision . s1 . body ) ; if ( ! collidingBodies . includes ( collision . s2 . body ) ) collidingBodies . push ( collision . s2 . body ) ; } collidingBodies . sort ( ( a , b ) => a . evaluationOrder - b . evaluationOrder ) ; for ( let i = 0 ; i < collidingBodies . length ; i ++ ) collidingBodies [ i ] . onBeforeCollisionResponse ( cumT , dt * t ) ; for ( let i = 0 ; i < collisions . length ; i ++ ) { let collision = collisions [ i ] ; if ( ( collision . s1 . collisionResponseMask & collision . s2 . collisionResponseMask ) === 0 ) continue ; CollisionResponse . solvePosition ( collision ) ; CollisionResponse . solveVelocity ( collision ) ; } for ( let i = 0 ; i < collidingBodies . length ; i ++ ) collidingBodies [ i ] . onAfterCollisionResponse ( cumT , dt * t ) ; this . inContactCcd = this . newInContactCcd ; this . newInContactCcd = new Set ( ) ; this . inContact = this . newInContact ; this . newInContact = new Set ( ) ; this . substep ( dt * ( 1 - t ) , cumT , depth + 1 ) ; } /** Computes all collision for a set of dynamic shapes. */ computeCollisions ( dynamicShapes , isCcdPass ) { let collisions = [ ] ; for ( let i = 0 ; i < dynamicShapes . length ; i ++ ) { let shape = dynamicShapes [ i ] ; if ( ! shape . body . enabled ) continue ; let broadphaseShape = shape . broadphaseShape || shape ; let collisionCandidates = this . cachedBroadphaseResults . get ( broadphaseShape ) ; let shapeCollisions = [ ] ; if ( ! collisionCandidates ) { collisionCandidates = this . octree . intersectAabb ( broadphaseShape . boundingBox ) as CollisionShape [ ] ; this . cachedBroadphaseResults . set ( broadphaseShape , collisionCandidates ) ; } outer : for ( let j = 0 ; j < collisionCandidates . length ; j ++ ) { let candidate = collisionCandidates [ j ] ; if ( shape === candidate ) continue ; if ( ( shape . collisionDetectionMask & candidate . collisionDetectionMask ) === 0 ) continue ; if ( ! candidate . body . enabled ) continue ; for ( let k = 0 ; k < collisions . length ; k ++ ) { let c = collisions [ k ] ; if ( c . s1 === candidate && c . s2 === shape ) continue outer ; } if ( isCcdPass ) { let timeOfImpact = CollisionDetection . determineTimeOfImpact ( shape , candidate ) ; if ( timeOfImpact === null ) continue ; let collision = new Collision ( shape , candidate ) ; collision . timeOfImpact = timeOfImpact ; this . newInContactCcd . add ( candidate ) ; collisions . push ( collision ) ; } else { let collides = CollisionDetection . checkIntersection ( shape , candidate ) ; if ( ! collides ) continue ; let collision = new Collision ( shape , candidate ) ; let isMainCollisionShape = ! ! ( shape . collisionDetectionMask & 1 ) ; if ( isMainCollisionShape ) { let collisionPlane = CollisionDetection . determineCollisionPlane ( p1 ) ; collision . supplyCollisionPlane ( collisionPlane ) ; } if ( isMainCollisionShape ) for ( let k = 0 ; k < shapeCollisions . length ; k ++ ) { let c2 = shapeCollisions [ k ] ; let distSq = collision . point2 . distanceToSquared ( c2 . point2 ) ; if ( distSq >= 0.1 ** 2 ) continue ; combinedCollisionShape . s1 = c2 . s2 ; combinedCollisionShape . s2 = candidate ; CollisionDetection . checkIntersection ( shape , combinedCollisionShape ) ; CollisionDetection . determineCollisionPlane ( p1 ) ; let combinedNormal = v3 . copy ( p1 . normal ) ; if ( collision . normal . dot ( combinedNormal ) <= 0 || c2 . normal . dot ( combinedNormal ) <= 0 ) break ; let size ; size = candidate . boundingBox . min . distanceTo ( candidate . boundingBox . max ) ; let hit1 = CollisionDetection . castRay ( candidate , singletonShape , candidate . getCenter ( v1 ) . addScaledVector ( combinedNormal , size ) , v2 . copy ( combinedNormal ) . negate ( ) , size ) ; size = c2 . s2 . boundingBox . min . distanceTo ( c2 . s2 . boundingBox . max ) ; let hit2 = CollisionDetection . castRay ( c2 . s2 , singletonShape , c2 . s2 . getCenter ( v1 ) . addScaledVector ( combinedNormal , size ) , v2 . copy ( combinedNormal ) . negate ( ) , size ) ; if ( hit1 && hit1 . normal . dot ( combinedNormal ) >= collision . normal . dot ( combinedNormal ) ) { collision . supplyCollisionPlane ( p1 . set ( hit1 . normal , collision . depth ) ) ; } if ( hit2 && hit2 . normal . dot ( combinedNormal ) >= c2 . normal . dot ( combinedNormal ) ) { c2 . supplyCollisionPlane ( p1 . set ( hit2 . normal , c2 . depth ) ) ; } break ; } collisions . push ( collision ) ; shapeCollisions . push ( collision ) ; shape . body . collisions . push ( collision ) ; collision . s2 . body . collisions . push ( collision ) ; this . newInContact . add ( candidate ) ; } } } for ( let i = 0 ; i < collisions . length ; i ++ ) collisions [ i ] . updateMaterialProperties ( ) ; return collisions ; } /** Casts a ray into the world and returns all intersections. */ castRay ( rayOrigin , rayDirection , lambdaMax , collisionDetectionMask = 0b1 ) { rayCastAabb . makeEmpty ( ) ; rayCastAabb . expandByPoint ( rayOrigin ) ; rayCastAabb . expandByPoint ( v1 . copy ( rayOrigin ) . addScaledVector ( rayDirection , lambdaMax ) ) ; let candidates = this . octree . intersectAabb ( rayCastAabb ) as CollisionShape [ ] ; let hits = [ ] ; for ( let candidate of candidates ) { if ( ( candidate . collisionDetectionMask & collisionDetectionMask ) === 0 ) continue ; let hit = CollisionDetection . castRay ( candidate , singletonShape , rayOrigin , rayDirection , lambdaMax ) ; if ( hit ) hits . push ( { ... hit , shape : candidate } ) ; } return hits . sort ( ( a , b ) => a . lambda - b . lambda ) ; } /** Performs convex casting of a given shape: Translates a shape from its current position linearly along a direction (swept volume) and returns all intersections with other shapes.  */ castShape ( shape , direction , lambdaMax ) { rayCastAabb . makeEmpty ( ) ; rayCastAabb . expandByPoint ( shape . boundingBox . min ) ; rayCastAabb . expandByPoint ( shape . boundingBox . max ) ; rayCastAabb . expandByPoint ( v1 . copy ( shape . boundingBox . min ) . addScaledVector ( direction , lambdaMax ) ) ; rayCastAabb . expandByPoint ( v1 . copy ( shape . boundingBox . max ) . addScaledVector ( direction , lambdaMax ) ) ; let candidates = this . octree . intersectAabb ( rayCastAabb ) as CollisionShape [ ] ; let hits = [ ] ; let negDirection = v2 . copy ( direction ) . negate ( ) ; for ( let candidate of candidates ) { if ( shape === candidate ) continue ; if ( ( shape . collisionDetectionMask & candidate . collisionDetectionMask ) === 0 ) continue ; if ( ! candidate . body . enabled ) continue ; if ( candidate . body . linearVelocity . lengthSq ( ) > 0 ) continue ; let hit = CollisionDetection . castRay ( shape , candidate , v1 . setScalar ( 0 ) , negDirection , lambdaMax ) ; if ( hit ) hits . push ( { ... hit , shape : candidate } ) ; } return hits . sort ( ( a , b ) => a . lambda - b . lambda ) ; } }