playcanvas
Version:
Open-source WebGL/WebGPU 3D engine for the web
292 lines (291 loc) • 9.78 kB
JavaScript
import { math } from "../../../core/math/math.js";
import { Mat4 } from "../../../core/math/mat4.js";
import { Quat } from "../../../core/math/quat.js";
import { Vec3 } from "../../../core/math/vec3.js";
import {
JOINTTYPE_6DOF,
JOINTTYPE_BALL,
JOINTTYPE_FIXED,
JOINTTYPE_HINGE,
JOINTTYPE_SLIDER,
MOTION_FREE,
MOTION_LIMITED
} from "../../components/joint/constants.js";
import { PhysicsJoint } from "../physics-joint.js";
const _frameMat = new Mat4();
const _vec3 = new Vec3();
const _quat = new Quat();
const _dof = { lower: 0, upper: 0 };
function dofLimits(motion, limits, scale) {
if (motion === MOTION_LIMITED) {
_dof.lower = limits.x * scale;
_dof.upper = limits.y * scale;
} else if (motion === MOTION_FREE) {
_dof.lower = 1;
_dof.upper = 0;
} else {
_dof.lower = 0;
_dof.upper = 0;
}
return _dof;
}
const fixedJoint = {
create(bodyA, bodyB, frameA, frameB) {
return new Ammo.btFixedConstraint(bodyA, bodyB, frameA, frameB);
}
};
const ballJoint = {
create(bodyA, bodyB, frameA, frameB) {
return new Ammo.btConeTwistConstraint(bodyA, bodyB, frameA, frameB);
},
updateLimits(joint, settings) {
const constraint = joint.nativeJoint;
if (settings.enableLimits) {
constraint.setLimit(5, settings.swingLimitY * math.DEG_TO_RAD);
constraint.setLimit(4, settings.swingLimitZ * math.DEG_TO_RAD);
constraint.setLimit(3, settings.twistLimit * math.DEG_TO_RAD);
} else {
constraint.setLimit(5, 1e30);
constraint.setLimit(4, 1e30);
constraint.setLimit(3, 1e30);
}
}
};
const hingeJoint = {
// bullet hinges rotate about frame Z - map the engine's X axis onto it
axisCorrection: new Mat4().setFromAxisAngle(Vec3.UP, 90),
create(bodyA, bodyB, frameA, frameB) {
return new Ammo.btHingeConstraint(bodyA, bodyB, frameA, frameB, false);
},
updateLimits(joint, settings) {
const constraint = joint.nativeJoint;
if (settings.enableLimits) {
const limits = settings.limits;
constraint.setLimit(limits.x * math.DEG_TO_RAD, limits.y * math.DEG_TO_RAD, 0.9, 0.3, 1);
} else {
constraint.setLimit(1, -1, 0.9, 0.3, 1);
}
},
updateMotor(joint, settings) {
const maxImpulse = settings.maxMotorForce * joint._world._fixedTimeStep;
joint.nativeJoint.enableAngularMotor(settings.maxMotorForce > 0, settings.motorSpeed * math.DEG_TO_RAD, maxImpulse);
}
};
const sliderJoint = {
create(bodyA, bodyB, frameA, frameB) {
return new Ammo.btSliderConstraint(bodyA, bodyB, frameA, frameB, true);
},
updateLimits(joint, settings) {
const constraint = joint.nativeJoint;
if (settings.enableLimits) {
const limits = settings.limits;
constraint.setLowerLinLimit(limits.x);
constraint.setUpperLinLimit(limits.y);
} else {
constraint.setLowerLinLimit(1);
constraint.setUpperLinLimit(-1);
}
constraint.setLowerAngLimit(0);
constraint.setUpperAngLimit(0);
},
updateMotor(joint, settings) {
const constraint = joint.nativeJoint;
constraint.setPoweredLinMotor(settings.maxMotorForce > 0);
constraint.setTargetLinMotorVelocity(settings.motorSpeed);
constraint.setMaxLinMotorForce(settings.maxMotorForce);
}
};
const sixDofJoint = {
create(bodyA, bodyB, frameA, frameB) {
return new Ammo.btGeneric6DofSpringConstraint(bodyA, bodyB, frameA, frameB, true);
},
updateLimits(joint, settings) {
const constraint = joint.nativeJoint;
let dof = dofLimits(settings.linearMotionX, settings.linearLimitsX, 1);
const lx = dof.lower, ux = dof.upper;
dof = dofLimits(settings.linearMotionY, settings.linearLimitsY, 1);
const ly = dof.lower, uy = dof.upper;
dof = dofLimits(settings.linearMotionZ, settings.linearLimitsZ, 1);
const lz = dof.lower, uz = dof.upper;
const limits = joint._world._btVec1;
limits.setValue(lx, ly, lz);
constraint.setLinearLowerLimit(limits);
limits.setValue(ux, uy, uz);
constraint.setLinearUpperLimit(limits);
dof = dofLimits(settings.angularMotionX, settings.angularLimitsX, math.DEG_TO_RAD);
const alx = dof.lower, aux = dof.upper;
dof = dofLimits(settings.angularMotionY, settings.angularLimitsY, math.DEG_TO_RAD);
const aly = dof.lower, auy = dof.upper;
dof = dofLimits(settings.angularMotionZ, settings.angularLimitsZ, math.DEG_TO_RAD);
const alz = dof.lower, auz = dof.upper;
limits.setValue(alx, aly, alz);
constraint.setAngularLowerLimit(limits);
limits.setValue(aux, auy, auz);
constraint.setAngularUpperLimit(limits);
},
updateSpring(joint, settings) {
const constraint = joint.nativeJoint;
const rad = math.DEG_TO_RAD;
const linStiffness = settings.linearStiffness;
const linDamping = settings.linearDamping;
const linEquilibrium = settings.linearEquilibrium;
const angStiffness = settings.angularStiffness;
const angDamping = settings.angularDamping;
const angEquilibrium = settings.angularEquilibrium;
constraint.enableSpring(0, linStiffness.x > 0);
constraint.setStiffness(0, linStiffness.x);
constraint.setDamping(0, linDamping.x);
constraint.setEquilibriumPoint(0, linEquilibrium.x);
constraint.enableSpring(1, linStiffness.y > 0);
constraint.setStiffness(1, linStiffness.y);
constraint.setDamping(1, linDamping.y);
constraint.setEquilibriumPoint(1, linEquilibrium.y);
constraint.enableSpring(2, linStiffness.z > 0);
constraint.setStiffness(2, linStiffness.z);
constraint.setDamping(2, linDamping.z);
constraint.setEquilibriumPoint(2, linEquilibrium.z);
constraint.enableSpring(3, angStiffness.x > 0);
constraint.setStiffness(3, angStiffness.x);
constraint.setDamping(3, angDamping.x);
constraint.setEquilibriumPoint(3, angEquilibrium.x * rad);
constraint.enableSpring(4, angStiffness.y > 0);
constraint.setStiffness(4, angStiffness.y);
constraint.setDamping(4, angDamping.y);
constraint.setEquilibriumPoint(4, angEquilibrium.y * rad);
constraint.enableSpring(5, angStiffness.z > 0);
constraint.setStiffness(5, angStiffness.z);
constraint.setDamping(5, angDamping.z);
constraint.setEquilibriumPoint(5, angEquilibrium.z * rad);
}
};
const jointImpls = {
[JOINTTYPE_FIXED]: fixedJoint,
[JOINTTYPE_BALL]: ballJoint,
[JOINTTYPE_HINGE]: hingeJoint,
[JOINTTYPE_SLIDER]: sliderJoint,
[JOINTTYPE_6DOF]: sixDofJoint
};
class AmmoPhysicsJoint extends PhysicsJoint {
_world;
_type;
_breakImpulse = Infinity;
constructor(world, type) {
super();
this._world = world;
this._type = type;
}
updateLimits(settings) {
const impl = jointImpls[this._type];
if (impl.updateLimits) {
impl.updateLimits(this, settings);
return true;
}
return false;
}
updateMotor(settings) {
const impl = jointImpls[this._type];
if (impl.updateMotor) {
impl.updateMotor(this, settings);
return true;
}
return false;
}
updateSpring(settings) {
const impl = jointImpls[this._type];
if (impl.updateSpring) {
impl.updateSpring(this, settings);
return true;
}
return false;
}
setBreakImpulse(impulse) {
this._breakImpulse = impulse;
const breakable = Number.isFinite(impulse);
this.nativeJoint.setBreakingImpulseThreshold(breakable ? impulse : Number.MAX_VALUE);
this.nativeJoint.enableFeedback(breakable);
}
isBroken() {
const constraint = this.nativeJoint;
if (typeof constraint.isEnabled === "function") {
return !constraint.isEnabled();
}
if (typeof constraint.getAppliedImpulse === "function") {
return constraint.getAppliedImpulse() >= this._breakImpulse;
}
return null;
}
}
function createBtFrame(frameMat, axisCorrection) {
_frameMat.copy(frameMat);
if (axisCorrection) {
_frameMat.mul(axisCorrection);
}
_frameMat.getTranslation(_vec3);
_quat.setFromMat4(_frameMat);
const frame = new Ammo.btTransform();
const origin = new Ammo.btVector3(_vec3.x, _vec3.y, _vec3.z);
const rotation = new Ammo.btQuaternion(_quat.x, _quat.y, _quat.z, _quat.w);
frame.setOrigin(origin);
frame.setRotation(rotation);
Ammo.destroy(origin);
Ammo.destroy(rotation);
return frame;
}
function getFixedBody(world) {
if (!world._fixedBody) {
const transform = new Ammo.btTransform();
transform.setIdentity();
const motionState = new Ammo.btDefaultMotionState(transform);
const shape = new Ammo.btSphereShape(1e-3);
const inertia = new Ammo.btVector3(0, 0, 0);
const info = new Ammo.btRigidBodyConstructionInfo(0, motionState, shape, inertia);
world._fixedBody = new Ammo.btRigidBody(info);
Ammo.destroy(info);
Ammo.destroy(inertia);
Ammo.destroy(transform);
}
return world._fixedBody;
}
function createJoint(world, desc) {
const impl = jointImpls[desc.type];
const axisCorrection = impl.axisCorrection;
const settings = desc.settings;
const frameA = createBtFrame(desc.frameA, axisCorrection);
const frameB = createBtFrame(desc.frameB, axisCorrection);
const bodyA = desc.bodyA.nativeBody;
const bodyB = desc.bodyB ? desc.bodyB.nativeBody : getFixedBody(world);
const constraint = impl.create(bodyA, bodyB, frameA, frameB);
Ammo.destroy(frameA);
Ammo.destroy(frameB);
const joint = new AmmoPhysicsJoint(world, desc.type);
joint.nativeJoint = constraint;
impl.updateLimits?.(joint, settings);
impl.updateMotor?.(joint, settings);
impl.updateSpring?.(joint, settings);
if (Number.isFinite(settings.breakImpulse)) {
joint.setBreakImpulse(settings.breakImpulse);
}
bodyA.activate();
bodyB.activate();
world.nativeWorld.addConstraint(constraint, !desc.enableCollision);
return joint;
}
function destroyJoint(world, joint) {
world.nativeWorld.removeConstraint(joint.nativeJoint);
Ammo.destroy(joint.nativeJoint);
joint.nativeJoint = null;
}
function destroyFixedBody(world) {
if (world._fixedBody) {
Ammo.destroy(world._fixedBody.getMotionState());
Ammo.destroy(world._fixedBody.getCollisionShape());
Ammo.destroy(world._fixedBody);
world._fixedBody = null;
}
}
export {
AmmoPhysicsJoint,
createJoint,
destroyFixedBody,
destroyJoint
};