import { Component } from "../core/Component" ; import { Vector3 } from "../math/Vector3" ; import { Collider } from "./Collider" ; import { CollisionGroup } from "./CollisionGroup" ; import { Triangle } from "../math/Triangle" ; import { Plane , PlaneClassification } from "../math/Plane" ; import { BoxCollisionShape } from "./BoxCollisionShape" ; import { SphereCollisionShape } from "./SphereCollisionShape" ; import { MeshCollisionShape } from "./MeshCollisionShape" ; import { MathEx } from "../math/MathEx" ; import { defaultAssets } from "../assets/DefaultAssets" ; import { VertexBuffer } from "../graphics/VertexBuffer" ; import { AssetReference } from "../serialization/AssetReference" ; import { Time } from "../core/Time" ; import * as Attributes from "../core/Attributes" ; import { Reference } from "../serialization/Reference" ; import { CollisionFilter } from "./CollisionFilter" ; import { SerializedObject } from "../core/SerializableObject" ; interface Collision { selfIntersectionPoint : Vector3 ; intersectionPoint : Vector3 ; toIntersection : number ; } namespace Private { let boxMesh ; let sphereMesh ; let collision = { selfIntersectionPoint : new Vector3 ( ) , intersectionPoint : new Vector3 ( ) , toIntersection : 0 } ; export function getLowestRoot ( a , b , c , maxR ) { let determinant = b * b - 4 * a * c ; if ( determinant < 0 ) { return null ; } let sqrtD = Math . sqrt ( determinant ) ; let r1 = ( - b - sqrtD ) / ( 2 * a ) ; let r2 = ( - b + sqrtD ) / ( 2 * a ) ; if ( r1 > r2 ) { let temp = r2 ; r2 = r1 ; r1 = temp ; } if ( r1 > 0 && r1 < maxR ) { return r1 ; } if ( r2 > 0 && r2 < maxR ) { return r2 ; } return null ; } function getBoxMesh ( ) { if ( ! boxMesh ) { boxMesh = defaultAssets . primitives . box . vertexBuffer ; } return boxMesh ; } function getSphereMesh ( ) { if ( ! sphereMesh ) { sphereMesh = defaultAssets . primitives . sphere . vertexBuffer ; } return sphereMesh ; } function detectCollision ( position , velocity , radius , colliders ) { let collisionValid = false ; collision . toIntersection = 999999 ; const boxVertices = getBoxMesh ( ) . attributes . position as number [ ] ; const sphereVertices = getSphereMesh ( ) . attributes . position as number [ ] ; const v1 = Vector3 . fromPool ( ) ; const v2 = Vector3 . fromPool ( ) ; const v3 = Vector3 . fromPool ( ) ; const triangle = Triangle . fromPool ( ) ; const plane = Plane . fromPool ( ) ; const toSelf = Vector3 . fromPool ( ) ; const selfIntersectionPoint = Vector3 . fromPool ( ) ; const intersectionPoint = Vector3 . fromPool ( ) ; const edge = Vector3 . fromPool ( ) ; const baseToVertex = Vector3 . fromPool ( ) ; const velocityLength = velocity . length ; for ( const collider of colliders ) { for ( const shape of collider . shapes ) { if ( ! shape ) { continue ; } let box = null ; let sphere = null ; let vertices ; if ( shape . isA ( BoxCollisionShape ) ) { box = shape as BoxCollisionShape ; vertices = boxVertices ; } else if ( shape . isA ( SphereCollisionShape ) ) { sphere = shape as SphereCollisionShape ; vertices = sphereVertices ; } else if ( shape . isA ( MeshCollisionShape ) ) { let mesh = ( shape as MeshCollisionShape ) . mesh . asset ; if ( ! mesh ) { continue ; } if ( mesh . vertexBuffer . primitiveType !== "TRIANGLES" ) { continue ; } if ( mesh . vertexBuffer . indices ) { continue ; } vertices = mesh . vertexBuffer . attributes . position as number [ ] ; } else { continue ; } const triangleCount = ( vertices . length / 3 ) / 3 ; const { worldMatrix } = collider . entity . transform ; for ( let i = 0 ; i < triangleCount ; ++ i ) { const index = i * 9 ; if ( box ) { v1 . set ( ( vertices [ index ] * box . extent . x ) + box . center . x , ( vertices [ index + 1 ] * box . extent . y ) + box . center . y , ( vertices [ index + 2 ] * box . extent . z ) + box . center . z ) . transform ( worldMatrix ) . divideVector ( radius ) ; v2 . set ( ( vertices [ index + 3 ] * box . extent . x ) + box . center . x , ( vertices [ index + 4 ] * box . extent . y ) + box . center . y , ( vertices [ index + 5 ] * box . extent . z ) + box . center . z ) . transform ( worldMatrix ) . divideVector ( radius ) ; v3 . set ( ( vertices [ index + 6 ] * box . extent . x ) + box . center . x , ( vertices [ index + 7 ] * box . extent . y ) + box . center . y , ( vertices [ index + 8 ] * box . extent . z ) + box . center . z ) . transform ( worldMatrix ) . divideVector ( radius ) ; } else if ( sphere ) { v1 . set ( ( vertices [ index ] * sphere . radius ) + sphere . center . x , ( vertices [ index + 1 ] * sphere . radius ) + sphere . center . y , ( vertices [ index + 2 ] * sphere . radius ) + sphere . center . z ) . transform ( worldMatrix ) . divideVector ( radius ) ; v2 . set ( ( vertices [ index + 3 ] * sphere . radius ) + sphere . center . x , ( vertices [ index + 4 ] * sphere . radius ) + sphere . center . y , ( vertices [ index + 5 ] * sphere . radius ) + sphere . center . z ) . transform ( worldMatrix ) . divideVector ( radius ) ; v3 . set ( ( vertices [ index + 6 ] * sphere . radius ) + sphere . center . x , ( vertices [ index + 7 ] * sphere . radius ) + sphere . center . y , ( vertices [ index + 8 ] * sphere . radius ) + sphere . center . z ) . transform ( worldMatrix ) . divideVector ( radius ) ; } else { v1 . set ( vertices [ index ] , vertices [ index + 1 ] , vertices [ index + 2 ] ) . transform ( worldMatrix ) . divideVector ( radius ) ; v2 . set ( vertices [ index + 3 ] , vertices [ index + 4 ] , vertices [ index + 5 ] ) . transform ( worldMatrix ) . divideVector ( radius ) ; v3 . set ( vertices [ index + 6 ] , vertices [ index + 7 ] , vertices [ index + 8 ] ) . transform ( worldMatrix ) . divideVector ( radius ) ; } plane . setFromPoints ( v1 , v2 , v3 ) ; const classification = plane . classifyPoint ( position ) ; if ( classification !== PlaneClassification . Front ) { continue ; } const nDotVelocity = plane . normal . dot ( velocity ) ; if ( nDotVelocity > 0 ) { continue ; } let t0 = - 1 ; let t1 = - 1 ; const distToPlane = plane . getSignedDistance ( position ) ; let embeddedInPlane = false ; if ( nDotVelocity !== 0 ) { t0 = ( 1 - distToPlane ) / nDotVelocity ; t1 = ( - 1 - distToPlane ) / nDotVelocity ; if ( t0 > t1 ) { let temp = t1 ; t1 = t0 ; t0 = temp ; } if ( t0 > 1 || t1 < 0 ) { continue ; } t0 = MathEx . clamp ( t0 , 0 , 1 ) ; t1 = MathEx . clamp ( t1 , 0 , 1 ) ; } else { if ( Math . abs ( distToPlane ) < 1 ) { t0 = 0 ; t1 = 1 ; embeddedInPlane = true ; } else { continue ; } } let foundCollision = false ; let collisionTime = 1 ; if ( ! embeddedInPlane ) { selfIntersectionPoint . copy ( plane . normal ) . flip ( ) . add ( position ) ; intersectionPoint . copy ( velocity ) . multiply ( t0 ) . add ( selfIntersectionPoint ) ; triangle . set ( v1 , v2 , v3 ) ; if ( triangle . contains ( intersectionPoint ) ) { foundCollision = true ; collisionTime = t0 ; } } if ( ! foundCollision ) { let velocitySquaredLength = velocity . lengthSq ; let a , b , c ; a = velocitySquaredLength ; toSelf . copy ( position ) . substract ( v1 ) ; b = 2 * velocity . dot ( toSelf ) ; c = toSelf . lengthSq - 1.0 ; let lowestRoot = Private . getLowestRoot ( a , b , c , collisionTime ) ; if ( lowestRoot !== null ) { collisionTime = lowestRoot ; foundCollision = true ; intersectionPoint . copy ( v1 ) ; } toSelf . copy ( position ) . substract ( v2 ) ; b = 2.0 * velocity . dot ( toSelf ) ; c = toSelf . lengthSq - 1.0 ; lowestRoot = Private . getLowestRoot ( a , b , c , collisionTime ) ; if ( lowestRoot !== null ) { collisionTime = lowestRoot ; foundCollision = true ; intersectionPoint . copy ( v2 ) ; } toSelf . copy ( position ) . substract ( v3 ) ; b = 2.0 * velocity . dot ( toSelf ) ; c = toSelf . lengthSq - 1.0 ; lowestRoot = Private . getLowestRoot ( a , b , c , collisionTime ) ; if ( lowestRoot !== null ) { collisionTime = lowestRoot ; foundCollision = true ; intersectionPoint . copy ( v3 ) ; } edge . substractVectors ( v2 , v1 ) ; baseToVertex . substractVectors ( v1 , position ) ; let edgeSquaredLength = edge . lengthSq ; let edgeDotVelocity = edge . dot ( velocity ) ; let edgeDotBaseToVertex = edge . dot ( baseToVertex ) ; a = edgeSquaredLength * - velocitySquaredLength + edgeDotVelocity * edgeDotVelocity ; b = edgeSquaredLength * ( 2 * velocity . dot ( baseToVertex ) ) - 2.0 * edgeDotVelocity * edgeDotBaseToVertex ; c = edgeSquaredLength * ( 1 - baseToVertex . lengthSq ) + edgeDotBaseToVertex * edgeDotBaseToVertex ; lowestRoot = Private . getLowestRoot ( a , b , c , collisionTime ) ; if ( lowestRoot !== null ) { let f = ( edgeDotVelocity * lowestRoot - edgeDotBaseToVertex ) / edgeSquaredLength ; if ( f >= 0.0 && f <= 1.0 ) { collisionTime = lowestRoot ; foundCollision = true ; intersectionPoint . copy ( edge ) . multiply ( f ) . add ( v1 ) ; } } edge . copy ( v3 ) . substract ( v2 ) ; baseToVertex . copy ( v2 ) . substract ( position ) ; edgeSquaredLength = edge . lengthSq ; edgeDotVelocity = edge . dot ( velocity ) ; edgeDotBaseToVertex = edge . dot ( baseToVertex ) ; a = edgeSquaredLength * - velocitySquaredLength + edgeDotVelocity * edgeDotVelocity ; b = edgeSquaredLength * ( 2 * velocity . dot ( baseToVertex ) ) - 2.0 * edgeDotVelocity * edgeDotBaseToVertex ; c = edgeSquaredLength * ( 1 - baseToVertex . lengthSq ) + edgeDotBaseToVertex * edgeDotBaseToVertex ; lowestRoot = Private . getLowestRoot ( a , b , c , collisionTime ) ; if ( lowestRoot !== null ) { let f = ( edgeDotVelocity * lowestRoot - edgeDotBaseToVertex ) / edgeSquaredLength ; if ( f >= 0.0 && f <= 1.0 ) { collisionTime = lowestRoot ; foundCollision = true ; intersectionPoint . copy ( edge ) . multiply ( f ) . add ( v2 ) ; } } edge . copy ( v1 ) . substract ( v3 ) ; baseToVertex . copy ( v3 ) . substract ( position ) ; edgeSquaredLength = edge . lengthSq ; edgeDotVelocity = edge . dot ( velocity ) ; edgeDotBaseToVertex = edge . dot ( baseToVertex ) ; a = edgeSquaredLength * - velocitySquaredLength + edgeDotVelocity * edgeDotVelocity ; b = edgeSquaredLength * ( 2 * velocity . dot ( baseToVertex ) ) - 2.0 * edgeDotVelocity * edgeDotBaseToVertex ; c = edgeSquaredLength * ( 1 - baseToVertex . lengthSq ) + edgeDotBaseToVertex * edgeDotBaseToVertex ; lowestRoot = Private . getLowestRoot ( a , b , c , collisionTime ) ; if ( lowestRoot !== null ) { let f = ( edgeDotVelocity * lowestRoot - edgeDotBaseToVertex ) / edgeSquaredLength ; if ( f >= 0.0 && f <= 1.0 ) { collisionTime = lowestRoot ; foundCollision = true ; intersectionPoint . copy ( edge ) . multiply ( f ) . add ( v3 ) ; } } } if ( foundCollision ) { const toIntersection = collisionTime * velocityLength ; if ( toIntersection < collision . toIntersection ) { collision . selfIntersectionPoint . copy ( selfIntersectionPoint ) ; collision . intersectionPoint . copy ( intersectionPoint ) ; collision . toIntersection = toIntersection ; collisionValid = true ; } } } } } return collisionValid ? collision : null ; } export function collisionDetectionAndResponse ( position , velocity , radius , colliders , positionOut ) { const skin = .001 ; const localPosition = Vector3 . fromPool ( ) . copy ( position ) . divideVector ( radius ) ; const localVelocity = Vector3 . fromPool ( ) . copy ( velocity ) . divideVector ( radius ) ; const toWorldSpace = ( local ) => local . multiplyVector ( radius ) ; const dest = Vector3 . fromPool ( ) . addVectors ( localPosition , localVelocity ) ; const firstPlane = Plane . fromPool ( ) ; const secondPlane = Plane . fromPool ( ) ; const normalizedVelocity = Vector3 . fromPool ( ) ; const slidingPlaneOrigin = Vector3 . fromPool ( ) ; const slidingPlaneNormal = Vector3 . fromPool ( ) ; const crease = Vector3 . fromPool ( ) ; const toDestination = Vector3 . fromPool ( ) ; for ( let i = 0 ; i < 3 ; ++ i ) { const velocityLength = localVelocity . length ; if ( MathEx . isZero ( velocityLength ) ) { positionOut . copy ( toWorldSpace ( dest ) ) ; return ; } const col = detectCollision ( localPosition , localVelocity , radius , colliders ) ; if ( ! col ) { positionOut . copy ( toWorldSpace ( dest ) ) ; return ; } normalizedVelocity . copy ( localVelocity ) . divide ( velocityLength ) ; const shortDist = Math . max ( col . toIntersection - skin , 0 ) ; localPosition . add ( normalizedVelocity . multiply ( shortDist ) ) ; if ( i === 0 ) { slidingPlaneOrigin . copy ( col . intersectionPoint ) ; slidingPlaneNormal . copy ( localPosition ) . substract ( col . intersectionPoint ) . normalize ( ) ; firstPlane . setFromPoint ( slidingPlaneNormal , slidingPlaneOrigin ) ; const longRadius = 1 + skin ; const d = firstPlane . getSignedDistance ( dest ) ; dest . substract ( slidingPlaneNormal . multiply ( d - longRadius ) ) ; localVelocity . substractVectors ( dest , localPosition ) ; } else if ( i === 1 ) { slidingPlaneOrigin . copy ( col . intersectionPoint ) ; slidingPlaneNormal . copy ( localPosition ) . substract ( col . intersectionPoint ) . normalize ( ) ; secondPlane . setFromPoint ( slidingPlaneNormal , slidingPlaneOrigin ) ; crease . crossVectors ( firstPlane . normal , secondPlane . normal ) . normalize ( ) ; const dist = toDestination . substractVectors ( dest , localPosition ) . dot ( crease ) ; localVelocity . copy ( crease ) . multiply ( dist ) ; dest . addVectors ( localPosition , localVelocity ) ; } } positionOut . copy ( toWorldSpace ( localPosition ) ) ; } } export class CharacterCollider extends Component { get version ( ) { return 2 ; } set gravity ( gravity ) { this . _gravity . copy ( gravity ) ; } get gravity ( ) { return this . _gravity ; } set radius ( radius ) { this . _radius . copy ( radius ) ; } get radius ( ) { return this . _radius ; } set desiredVelocity ( velocity ) { this . _desiredVelocity . copy ( velocity ) ; } get velocity ( ) { return this . _velocity ; } get group ( ) { return this . _group . asset ; } private _gravity = new Vector3 ( 0 , - 40 , 0 ) ; private _radius = new Vector3 ( 1 , 1 , 1 ) ; private _group = new AssetReference ( CollisionGroup ) ; private _filter = new Reference ( CollisionFilter ) ; @ Attributes . unserializable ( ) private _velocity = new Vector3 ( ) ; @ Attributes . unserializable ( ) private _desiredVelocity = new Vector3 ( ) ; update ( _colliders ) { const filter = this . _filter . instance ; const colliders = filter ? _colliders . filter ( c => filter . canCollideWith ( c . group ) ) : _colliders ; const { worldPosition , worldScale , worldUp } = this . entity . transform ; const radius = Vector3 . fromPool ( ) . copy ( this . radius ) . multiplyVector ( worldScale ) ; const offset = Vector3 . fromPool ( ) . copy ( worldUp ) . multiply ( radius . y ) ; const position = Vector3 . fromPool ( ) . copy ( worldPosition ) . add ( offset ) ; const positionOut = Vector3 . fromPool ( ) ; const previousVerticalVelocity = Vector3 . fromPool ( ) . copy ( this . _velocity ) . projectOnVector ( this . _gravity ) ; const verticalVelocity = Vector3 . fromPool ( ) . copy ( this . _desiredVelocity ) . projectOnVector ( this . _gravity ) ; const planarVelocity = Vector3 . fromPool ( ) . substractVectors ( this . _desiredVelocity , verticalVelocity ) ; this . _velocity . copy ( planarVelocity ) ; const velocity = Vector3 . fromPool ( ) . copy ( this . _velocity ) . multiply ( Time . deltaTime ) ; Private . collisionDetectionAndResponse ( position , velocity , radius , colliders , positionOut ) ; if ( verticalVelocity . lengthSq > 0 ) { this . _velocity . copy ( verticalVelocity ) ; } else { this . _velocity . copy ( previousVerticalVelocity ) ; } this . _velocity . add ( Vector3 . fromPool ( ) . copy ( this . _gravity ) . multiply ( Time . deltaTime ) ) ; velocity . copy ( this . _velocity ) . multiply ( Time . deltaTime ) ; position . copy ( positionOut ) ; Private . collisionDetectionAndResponse ( position , velocity , radius , colliders , positionOut ) ; this . _velocity . substractVectors ( positionOut , position ) . divide ( Time . deltaTime ) ; this . entity . transform . worldPosition = positionOut . substract ( offset ) ; } upgrade ( json , previousVersion ) { if ( previousVersion === 1 ) { Object . assign ( json . properties , { _gravity : { _x : 0 , _y : json . properties . gravity , _z : 0 } , _radius : json . properties . radius } ) ; delete json . properties . gravity ; delete json . properties . radius ; } return json ; } }