Skip to content

Commit

Permalink
Merge pull request #37 from LeXXik/collide-point
Browse files Browse the repository at this point in the history 
Add collide point query
  • Loading branch information
@yohami
yohami authored May 20, 2024
2 parents 09333aa + 8e1a776 commit 5e71217
Show file tree
Hide file tree
Showing 5 changed files with 144 additions and 23 deletions.
79 changes: 79 additions & 0 deletions src/physics/jolt/back/operators/querier.mjs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,11 @@ import { Debug } from '../../debug.mjs';
import {
BUFFER_READ_BOOL, BUFFER_READ_UINT32, BUFFER_READ_UINT8, BUFFER_WRITE_BOOL,
BUFFER_WRITE_JOLTVEC32, BUFFER_WRITE_UINT16, BUFFER_WRITE_UINT32, CMD_CAST_RAY, CMD_CAST_SHAPE,
CMD_COLLIDE_POINT,
COMPONENT_SYSTEM_MANAGER
} from '../../constants.mjs';

let collidePointResult;
let params = [];

class Querier {
Expand All @@ -27,6 +29,7 @@ class Querier {

this._collectorRayFirst = new Jolt.CastRayClosestHitCollisionCollector();
this._collectorRayAll = new Jolt.CastRayAllHitCollisionCollector();
this._collectorPoint = null;

this._collectorShapeFirst = new Jolt.CastShapeClosestHitCollisionCollector();
this._collectorShapeAll = new Jolt.CastShapeAllHitCollisionCollector();
Expand All @@ -48,6 +51,10 @@ class Querier {
ok = this._castShape(cb);
break;

case CMD_COLLIDE_POINT:
ok = this._collidePoint(cb);
break;

default:
if ($_DEBUG) {
Debug.error(`Invalid querier command: ${command}`);
Expand Down Expand Up @@ -79,6 +86,11 @@ class Querier {
Jolt.destroy(this._collectorRayAll);
this._collectorRayAll = null;

if (this._collectorPoint) {
Jolt.destroy(this._collectorPoint);
this._collectorPoint = null;
}

this._commandsBuffer.destroy();
this._commandsBuffer = null;

Expand Down Expand Up @@ -270,6 +282,73 @@ class Querier {
return true;
}

_collidePoint(cb) {
const backend = this._backend;
const jv = this._tempVectors[1];
const buffer = backend.outBuffer;
const tracker = backend.tracker;
const system = backend.physicsSystem;
const Jolt = backend.Jolt;
const joltInterface = backend.joltInterface;
const { bodyFilter, shapeFilter } = backend;

buffer.writeOperator(COMPONENT_SYSTEM_MANAGER);
buffer.writeCommand(CMD_COLLIDE_POINT);

const queryIndex = cb.read(BUFFER_READ_UINT32);
buffer.write(queryIndex, BUFFER_WRITE_UINT16, false);

if (!collidePointResult) {
collidePointResult = [];
}
collidePointResult.length = 0;

let collector = this._collectorPoint;
if (!collector) {
collector = this._collectorPoint = new Jolt.CollidePointCollectorJS();
collector.Reset = function () {
collector.ResetEarlyOutFraction();
};
collector.AddHit = function () {};
collector.OnBody = function (bodyPointer) {
collidePointResult.push(tracker.getPCID(bodyPointer));
};
}

const customBPFilter = cb.flag;
const bpFilter = customBPFilter ? new Jolt.DefaultBroadPhaseLayerFilter(joltInterface.GetObjectVsBroadPhaseLayerFilter(), cb.read(BUFFER_READ_UINT32)) : backend.bpFilter;

const customObjFilter = cb.flag;
const objFilter = customObjFilter ? new Jolt.DefaultObjectLayerFilter(joltInterface.GetObjectLayerPairFilter(), cb.read(BUFFER_READ_UINT32)) : backend.objFilter;

try {
jv.FromBuffer(cb);
system.GetNarrowPhaseQuery().CollidePoint(jv, collector, bpFilter, objFilter, bodyFilter, shapeFilter);
collector.Reset();

if (customBPFilter) {
Jolt.destroy(customBPFilter);
}

if (customObjFilter) {
Jolt.destroy(customObjFilter);
}
} catch (e) {
if ($_DEBUG) {
Debug.error(e);
}
return false;
}

const count = collidePointResult.length;
buffer.write(count, BUFFER_WRITE_UINT16, false);
for (let i = 0; i < count; i++) {
buffer.write(collidePointResult[i], BUFFER_WRITE_UINT32, false);
}

return true;
}

static writeRayHit(buffer, system, tracker, cast, calculateNormal, hit, Jolt) {
const body = system.GetBodyLockInterfaceNoLock().TryGetBody(hit.mBodyID);
const point = cast.GetPointOnRay(hit.mFraction);
Expand Down
2 changes: 2 additions & 0 deletions src/physics/jolt/constants.mjs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -166,6 +166,8 @@ export const CMD_SET_OBJ_LAYER = 34;
export const CMD_SET_GRAVITY_FACTOR = 35;
export const CMD_SET_DOF = 36;

export const CMD_COLLIDE_POINT = 37;

// Constraints 500+

export const CMD_JNT_SET_ENABLED = 500;
Expand Down
10 changes: 1 addition & 9 deletions src/physics/jolt/front/body/system.mjs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,7 @@ import { ResponseHandler } from '../response-handler.mjs';
import { ShapeComponentSystem } from '../shape/system.mjs';
import { BodyComponent } from './component.mjs';
import {
CMD_CAST_RAY, CMD_CAST_SHAPE, CMD_CREATE_BODY, CMD_REPORT_CONTACTS,
CMD_REPORT_TRANSFORMS, OPERATOR_CREATOR
CMD_CREATE_BODY, CMD_REPORT_CONTACTS, CMD_REPORT_TRANSFORMS, OPERATOR_CREATOR
} from '../../constants.mjs';

const schema = [
Expand Down Expand Up @@ -116,13 +115,6 @@ class BodyComponentSystem extends ShapeComponentSystem {
ShapeComponentSystem.updateDynamic(cb);
break;

// TODO
// handle by manager directly
case CMD_CAST_RAY:
case CMD_CAST_SHAPE:
ResponseHandler.handleQuery(cb, this.entityMap, this._manager.queryMap);
break;

case CMD_REPORT_CONTACTS:
ResponseHandler.handleContact(cb, this.entityMap, this._manager.config);
break;
Expand Down
46 changes: 34 additions & 12 deletions src/physics/jolt/front/response-handler.mjs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -161,28 +161,28 @@ class ResponseHandler {
}
}

static handleQuery(buffer, queryMap) {
const queryIndex = buffer.read(BUFFER_READ_UINT16);
const firstOnly = buffer.read(BUFFER_READ_BOOL);
const hitsCount = buffer.read(BUFFER_READ_UINT16);
static handleCastQuery(cb, queryMap) {
const queryIndex = cb.read(BUFFER_READ_UINT16);
const firstOnly = cb.read(BUFFER_READ_BOOL);
const hitsCount = cb.read(BUFFER_READ_UINT16);

let result = firstOnly ? null : [];

for (let i = 0; i < hitsCount; i++) {
const bodyIndex = buffer.read(BUFFER_READ_UINT32);
const bodyIndex = cb.read(BUFFER_READ_UINT32);

const point = new Vec3(
buffer.read(BUFFER_READ_FLOAT32),
buffer.read(BUFFER_READ_FLOAT32),
buffer.read(BUFFER_READ_FLOAT32)
cb.read(BUFFER_READ_FLOAT32),
cb.read(BUFFER_READ_FLOAT32),
cb.read(BUFFER_READ_FLOAT32)
);

let normal;
if (buffer.flag) {
if (cb.flag) {
normal = new Vec3(
buffer.read(BUFFER_READ_FLOAT32),
buffer.read(BUFFER_READ_FLOAT32),
buffer.read(BUFFER_READ_FLOAT32)
cb.read(BUFFER_READ_FLOAT32),
cb.read(BUFFER_READ_FLOAT32),
cb.read(BUFFER_READ_FLOAT32)
);
}

Expand All @@ -207,6 +207,28 @@ class ResponseHandler {
callback?.(result);
}

static handleCollidePointQuery(cb, queryMap) {
const queryIndex = cb.read(BUFFER_READ_UINT16);
const hitsCount = cb.read(BUFFER_READ_UINT16);

const result = [];

for (let i = 0; i < hitsCount; i++) {
const bodyIndex = cb.read(BUFFER_READ_UINT32);

const entity = ShapeComponentSystem.entityMap.get(bodyIndex);
if (!entity) {
continue;
}

result.push(entity);
}

const callback = queryMap.get(queryIndex);
queryMap.free(queryIndex);
callback?.(result);
}

static handleCharSetShape(cb, queryMap) {
const cbIndex = cb.read(BUFFER_READ_UINT32);
const callback = queryMap.get(cbIndex);
Expand Down
30 changes: 28 additions & 2 deletions src/physics/jolt/manager.mjs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ import { VehicleComponentSystem } from './front/vehicle/system.mjs';
import { Quat, Vec3, Color, LAYERID_IMMEDIATE } from 'playcanvas';
import {
BUFFER_WRITE_BOOL, BUFFER_WRITE_UINT16, BUFFER_WRITE_UINT32, BUFFER_WRITE_UINT8,
BUFFER_WRITE_VEC32, CMD_CAST_RAY, CMD_CAST_SHAPE, CMD_CHANGE_GRAVITY, CMD_CREATE_GROUPS,
BUFFER_WRITE_VEC32, CMD_CAST_RAY, CMD_CAST_SHAPE, CMD_CHANGE_GRAVITY, CMD_COLLIDE_POINT, CMD_CREATE_GROUPS,
CMD_CREATE_SHAPE, CMD_DESTROY_SHAPE, CMD_TOGGLE_GROUP_PAIR, COMPONENT_SYSTEM_BODY,
COMPONENT_SYSTEM_CHAR, COMPONENT_SYSTEM_CONSTRAINT, COMPONENT_SYSTEM_MANAGER,
COMPONENT_SYSTEM_SOFT_BODY, COMPONENT_SYSTEM_VEHICLE, OPERATOR_CLEANER, OPERATOR_CREATOR,
Expand Down Expand Up @@ -136,7 +136,10 @@ class JoltManager extends PhysicsManager {
switch (command) {
case CMD_CAST_RAY:
case CMD_CAST_SHAPE:
ResponseHandler.handleQuery(cb, this._queryMap);
ResponseHandler.handleCastQuery(cb, this._queryMap);
break;
case CMD_COLLIDE_POINT:
ResponseHandler.handleCollidePointQuery(cb, this._queryMap);
break;
}
}
Expand Down Expand Up @@ -260,6 +263,8 @@ class JoltManager extends PhysicsManager {
if (ok && opts?.calculateNormal != null) ok = Debug.checkBool(opts.calculateNormal);
if (ok && opts?.ignoreBackFaces != null) ok = Debug.checkBool(opts.ignoreBackFaces);
if (ok && opts?.treatConvexAsSolid != null) ok = Debug.checkBool(opts.treatConvexAsSolid);
if (ok && opts?.bpFilterLayer != null) ok = Debug.checkUint(opts.bpFilterLayer);
if (ok && opts?.objFilterLayer != null) ok = Debug.checkUint(opts.objFilterLayer);
if (!ok) {
return;
}
Expand Down Expand Up @@ -323,6 +328,27 @@ class JoltManager extends PhysicsManager {
cb.write(opts?.bpFilterLayer, BUFFER_WRITE_UINT32);
cb.write(opts?.objFilterLayer, BUFFER_WRITE_UINT32);
}

collidePoint(point, callback, opts) {
if ($_DEBUG) {
let ok = Debug.checkVec(point, `Invalid point vector`);
if (ok && opts?.bpFilterLayer != null) ok = Debug.checkUint(opts.bpFilterLayer);
if (ok && opts?.objFilterLayer != null) ok = Debug.checkUint(opts.objFilterLayer);
if (!ok) {
return;
}
}

const cb = this._outBuffer;
const queryIndex = this._queryMap.add(callback);

cb.writeOperator(OPERATOR_QUERIER);
cb.writeCommand(CMD_COLLIDE_POINT);
cb.write(queryIndex, BUFFER_WRITE_UINT32, false);
cb.write(opts?.bpFilterLayer, BUFFER_WRITE_UINT32);
cb.write(opts?.objFilterLayer, BUFFER_WRITE_UINT32);
cb.write(point, BUFFER_WRITE_VEC32, false);
}
}

export { JoltManager };

0 comments on commit 5e71217

Please sign in to comment.