Added support for shapeCast and merged raycastFirst and raycastAll into raycast

src/framework/components/rigid-body/system.js

@@ -3,15 +3,24 @@ import { ObjectPool } from '../../../core/object-pool.js';
 import { Debug } from '../../../core/debug.js';
 
 import { Vec3 } from '../../../core/math/vec3.js';
+import { Quat } from '../../../core/math/quat.js';
+import { Mat4 } from '../../../core/math/mat4.js';
 
 import { Component } from '../component.js';
 import { ComponentSystem } from '../system.js';
 
-import { BODYFLAG_NORESPONSE_OBJECT } from './constants.js';
+import { BODYFLAG_NORESPONSE_OBJECT, BODYSTATE_ACTIVE_TAG, BODYSTATE_DISABLE_DEACTIVATION } from './constants.js';
 import { RigidBodyComponent } from './component.js';
 import { RigidBodyComponentData } from './data.js';
 
-let ammoRayStart, ammoRayEnd;
+// Ammo.js variable for performance saving.
+let ammoRayStart, ammoRayEnd, ammoVec3, ammoQuat, ammoTransform;
+
+// RigidBody for shape tests. Permanent to save performance.
+let shapeTestBody;
+const shapecastPosition = new Vec3();
+const shapecastRotation = new Quat();
+const shapecastRotationMatrix = new Mat4();
 
 /**
  * Object holding the result of a successful raycast hit.
@@ -227,6 +236,42 @@ class ContactResult {
  }
 }
 
+/**
+ * Object holding the result of a hit on an Entity.
+ */
+class HitResult {
+ /**
+ * Create a new HitResult instance.
+ *
+ * @param {import('../../entity.js').Entity} entity - The entity that was hit.
+ * @param {Vec3} point - The point on the entity where the hit occurred, in world space. When returned by a shapecast it is the first point found to collide, it does not have to be the closest.
+ * @param {Vec3} normal - The normal vector of the hit on the entity, in world space.
+ * @hideconstructor
+ */
+ constructor(entity, point, normal) {
+ /**
+ * The entity that was hit.
+ *
+ * @type {import('../../entity.js').Entity}
+ */
+ this.entity = entity;
+
+ /**
+ * The point on the entity where the hit occurred, in world space. When returned by a shapecast it is the first point found to collide, it does not have to be the closest.
+ *
+ * @type {Vec3}
+ */
+ this.point = point;
+
+ /**
+ * The normal vector of the hit on the entity, in world space.
+ *
+ * @type {Vec3}
+ */
+ this.normal = normal;
+ }
+}
+
 const _schema = ['enabled'];
 
 /**
@@ -347,6 +392,10 @@ class RigidBodyComponentSystem extends ComponentSystem {
  // Lazily create temp vars
  ammoRayStart = new Ammo.btVector3();
  ammoRayEnd = new Ammo.btVector3();
+ ammoVec3 = new Ammo.btVector3();
+ ammoQuat = new Ammo.btQuaternion();
+ ammoTransform = new Ammo.btTransform();
+
  RigidBodyComponent.onLibraryLoaded();
 
  this.contactPointPool = new ObjectPool(ContactPoint, 1);
@@ -552,6 +601,275 @@ class RigidBodyComponentSystem extends ComponentSystem {
  return results;
  }
 
+ /**
+ * Perform a collision check on the world and return all entities the box hits.
+ * It returns an array of {@link HitResult}. If no hits are
+ * detected, the returned array will be of length 0.
+ *
+ * @param {Vec3} halfExtents - The half-extents of the box in the x, y and z axes. Cast distance will be added to Z-Axis.
+ * @param {Vec3} start - The world space point where the box starts.
+ * @param {Vec3} end - The world space point where the box ends.
+ *
+ * @returns {HitResult[]} An array of boxCast hit results (0 length if there were no hits).
+ */
+ boxCastAll(halfExtents, start, end) {
+ // Sweeping
+ ammoVec3.setValue(halfExtents.x, halfExtents.y, halfExtents.z + start.distance(end) / 2);
+
+ // Find rotation
+ shapecastRotationMatrix.setLookAt(start, end, Vec3.UP);
+ shapecastRotation.setFromMat4(shapecastRotationMatrix);
+
+ // Transform start vector to make it beween initial start and end.
+ shapecastPosition.lerp(start, end, 0.5);
+
+ return this._shapeTest(new Ammo.btBoxShape(ammoVec3), shapecastPosition, shapecastRotation);
+ }
+
+ /**
+ * Perform a collision check on the world and return all entities the sphere hits.
+ * It returns an array of {@link HitResult}. If no hits are
+ * detected, the returned array will be of length 0.
+ *
+ * @param {number} radius - The radius for the sphere.
+ * @param {Vec3} start - The world space point for the center of the sphere at the beginning of the cast.
+ * @param {Vec3} end - The world space point for the center of the sphere at the end of the cast.
+ *
+ * @returns {HitResult[]} An array of sphereCast hit results (0 length if there were no hits).
+ */
+ sphereCastAll(radius, start, end) {
+ // Sweeping
+ const height = start.distance(end) + radius;
+
+ // Find rotation
+ shapecastRotationMatrix.setLookAt(start, end, Vec3.UP);
+ shapecastRotation.setFromMat4(shapecastRotationMatrix);
+
+ // Transform start vector to make it beween initial start and end.
+ shapecastPosition.lerp(start, end, 0.5);
+
+ return this._shapeTest(new Ammo.btCapsuleShapeZ(radius, height), shapecastPosition, shapecastRotation);
+ }
+
+ /**
+ * Perform a collision check on the world and return all entities the shape hits.
+ * It returns an array of {@link HitResult}. If no hits are
+ * detected, the returned array will be of length 0.
+ *
+ * @param {object} shape - The shape to use for collision.
+ * @param {number} shape.axis - The local space axis with which the capsule, cylinder or cone shape's length is aligned. 0 for X, 1 for Y and 2 for Z. Defaults to 1 (Y-axis).
+ * @param {Vec3} shape.halfExtents - The half-extents of the box in the x, y and z axes.
+ * @param {number} shape.height - The total height of the capsule, cylinder or cone from tip to tip.
+ * @param {string} shape.type - The type of shape to use. Available options are "box", "capsule", "cone", "cylinder" or "sphere". Defaults to "box".
+ * @param {number} shape.radius - The radius of the sphere, capsule, cylinder or cone.
+ * @param {Vec3} [position] - The world space position for the shape to be.
+ * @param {Vec3|Quat} [rotation] - The world space rotation for the shape to have.
+ *
+ * @returns {HitResult[]} An array of shapeTest hit results (0 length if there were no hits).
+ */
+ shapeTest(shape, position, rotation) {
+ switch (shape.type) {
+ case 'capsule':
+ return this.capsuleTest(shape.radius, shape.height, shape.axis, position, rotation);
+ case 'cone':
+ return this.coneTest(shape.radius, shape.height, shape.axis, position, rotation);
+ case 'cylinder':
+ return this.cylinderTest(shape.radius, shape.height, shape.axis, position, rotation);
+ case 'sphere':
+ return this.sphereTest(shape.radius, position, rotation);
+ default:
+ return this.boxTest(shape.halfExtents, position, rotation);
+ }
+ }
+
+ /**
+ * Perform a collision check on the world and return all entities the box hits.
+ * It returns an array of {@link HitResult}. If no hits are
+ * detected, the returned array will be of length 0.
+ *
+ * @param {Vec3} halfExtents - The half-extents of the box in the x, y and z axes.
+ * @param {Vec3} [position] - The world space position for the box to be.
+ * @param {Vec3|Quat} [rotation] - The world space rotation for the box to have.
+ *
+ * @returns {HitResult[]} An array of boxTest hit results (0 length if there were no hits).
+ */
+ boxTest(halfExtents, position, rotation) {
+ ammoVec3.setValue(halfExtents.x, halfExtents.y, halfExtents.z);
+ return this._shapeTest(new Ammo.btBoxShape(ammoVec3), position, rotation);
+ }
+
+ /**
+ * Perform a collision check on the world and return all entities the capsule hits.
+ * It returns an array of {@link HitResult}. If no hits are
+ * detected, the returned array will be of length 0.
+ *
+ * @param {number} radius - The radius of the capsule.
+ * @param {number} height - The total height of the capsule from tip to tip.
+ * @param {number} axis - The local space axis with which the capsule's length is aligned. 0 for X, 1 for Y and 2 for Z. Defaults to 1 (Y-axis).
+ * @param {Vec3} [position] - The world space position for the capsule to be.
+ * @param {Vec3|Quat} [rotation] - The world space rotation for the capsule to have.
+ *
+ * @returns {HitResult[]} An array of capsuletest hit results (0 length if there were no hits).
+ */
+ capsuleTest(radius, height, axis, position, rotation) {
+ let fn = 'btCapsuleShape';
+
+ if (axis === 0) {
+ fn = 'btCapsuleShapeX';
+ } else if (axis === 2) {
+ fn = 'btCapsuleShapeZ';
+ }
+
+ return this._shapeTest(new Ammo[fn](radius, height), position, rotation);
+ }
+
+ /**
+ * Perform a collision check on the world and return all entities the cone hits.
+ * It returns an array of {@link HitResult}. If no hits are
+ * detected, the returned array will be of length 0.
+ *
+ * @param {number} radius - The radius of the cone.
+ * @param {number} height - The total height of the cone from tip to tip.
+ * @param {number} axis - The local space axis with which the cone's length is aligned. 0 for X, 1 for Y and 2 for Z. Defaults to 1 (Y-axis).
+ * @param {Vec3} [position] - The world space position for the cone to be.
+ * @param {Vec3|Quat} [rotation] - The world space rotation for the cone to have.
+ *
+ * @returns {HitResult[]} An array of conetest hit results (0 length if there were no hits).
+ */
+ coneTest(radius, height, axis, position, rotation) {
+ let fn = 'btConeShape';
+
+ if (axis === 0) {
+ fn = 'btConeShapeX';
+ } else if (axis === 2) {
+ fn = 'btConeShapeZ';
+ }
+
+ return this._shapeTest(new Ammo[fn](radius, height), position, rotation);
+ }
+
+ /**
+ * Perform a collision check on the world and return all entities the cylinder hits.
+ * It returns an array of {@link HitResult}. If no hits are
+ * detected, the returned array will be of length 0.
+ *
+ * @param {number} radius - The radius of the cylinder.
+ * @param {number} height - The total height of the cylinder from tip to tip.
+ * @param {number} axis - The local space axis with which the cylinder's length is aligned. 0 for X, 1 for Y and 2 for Z. Defaults to 1 (Y-axis).
+ * @param {Vec3} [position] - The world space position for the cylinder to be.
+ * @param {Vec3|Quat} [rotation] - The world space rotation for the cylinder to have.
+ *
+ * @returns {HitResult[]} An array of cylinderTest hit results (0 length if there were no hits).
+ */
+ cylinderTest(radius, height, axis, position, rotation) {
+ let fn = 'btCylinderShape';
+
+ if (axis === 0) {
+ fn = 'btCylinderShapeX';
+ } else if (axis === 2) {
+ fn = 'btCylinderShapeZ';
+ }
+
+ return this._shapeTest(new Ammo[fn](radius, height), position, rotation);
+ }
+
+ /**
+ * Perform a collision check on the world and return all entities the sphere hits.
+ * It returns an array of {@link HitResult}. If no hits are
+ * detected, the returned array will be of length 0.
+ *
+ * @param {number} radius - The radius of the sphere.
+ * @param {Vec3} [position] - The world space position for the sphere to be.
+ * @param {Vec3|Quat} [rotation] - The world space rotation for the sphere to have.
+ *
+ * @returns {HitResult[]} An array of sphereTest hit results (0 length if there were no hits).
+ */
+ sphereTest(radius, position, rotation) {
+ return this._shapeTest(new Ammo.btSphereShape(radius), position, rotation);
+ }
+
+ /**
+ * Perform a collision check on the world and return all entities the shape hits.
+ * It returns an array of {@link HitResult}. If no hits are
+ * detected, the returned array will be of length 0.
+ *
+ * @param {Ammo.btCollisionShape} shape - The Ammo.btCollisionShape to use for collision check.
+ * @param {Vec3} [position] - The world space position for the shape to be.
+ * @param {Vec3|Quat} [rotation] - The world space rotation for the shape to have.
+ *
+ * @returns {HitResult[]} An array of shapeTest hit results (0 length if there were no hits).
+ * @private
+ */
+ _shapeTest(shape, position = Vec3.ZERO, rotation = Vec3.ZERO) {
+ Debug.assert(Ammo.ConcreteContactResultCallback, 'pc.RigidBodyComponentSystem#_shapecast: Your version of ammo.js does not expose Ammo.ConcreteContactResultCallback. Update it to latest.');
+
+ const results = [];
+
+ // Set proper position
+ ammoVec3.setValue(position.x, position.y, position.z);
+
+ // Set proper rotation
+ if (rotation instanceof Quat) {
+ ammoQuat.setValue(rotation.x, rotation.y, rotation.z, rotation.w);
+ } else {
+ ammoQuat.setEulerZYX(rotation.z, rotation.y, rotation.x);
+ }
+
+ // Assign position and rotation to transform.
+ ammoTransform.setIdentity();
+ ammoTransform.setOrigin(ammoVec3);
+ ammoTransform.setRotation(ammoQuat);
+
+ // We only initialize the shapeTast body here so we don't have an extra body unless the user uses this function
+ if (!shapeTestBody) {
+ shapeTestBody = this.createBody(0, shape, ammoTransform);
+ }
+
+ // Make sure the body has proper shape, transform and is active.
+ shapeTestBody.setCollisionShape(shape);
+ shapeTestBody.setWorldTransform(ammoTransform);
+ shapeTestBody.forceActivationState(BODYSTATE_ACTIVE_TAG);
+
+ // Callback for the contactTest results.
+ const resultCallback = new Ammo.ConcreteContactResultCallback();
+ resultCallback.addSingleResult = function (cp, colObj0Wrap, partId0, index0, colObj1Wrap, p1, index1) {
+ // Retrieve collided entity.
+ const body1 = Ammo.castObject(Ammo.wrapPointer(colObj1Wrap, Ammo.btCollisionObjectWrapper).getCollisionObject(), Ammo.btRigidBody);
+
+ // Make sure there is an existing entity.
+ if (body1.entity) {
+ // Retrieve manifold point.
+ const manifold = Ammo.wrapPointer(cp, Ammo.btManifoldPoint);
+
+ // Make sure there is a collision
+ if (manifold.getDistance() < 0) {
+ const point = manifold.get_m_positionWorldOnB();
+ const normal = manifold.get_m_normalWorldOnB();
+
+ // Push the result.
+ results.push(new HitResult(
+ body1.entity,
+ new Vec3(point.x(), point.y(), point.z()),
+ new Vec3(normal.x(), normal.y(), normal.z())
+ ));
+ }
+ }
+ };
+
+ // Check for contacts.
+ this.dynamicsWorld.contactTest(shapeTestBody, resultCallback);
+
+ // Disable body and remove shape.
+ shapeTestBody.forceActivationState(BODYSTATE_DISABLE_DEACTIVATION);
+ shapeTestBody.setCollisionShape(null);
+
+ // Destroy unused variables for performance.
+ Ammo.destroy(resultCallback);
+ Ammo.destroy(shape);
+
+ return results;
+ }
+
  /**
  * Stores a collision between the entity and other in the contacts map and returns true if it
  * is a new collision.