I tried incorporating some parts from Bullet and PhysX into debug visualizations for Havok, though haven’t yet figured out how to draw a pyramid nicely
It’s hard to move all the Havok code from the video to a PG. In the meantime, I’ve pasted the joint limit debug visualization part below:
Havok Joint Limit Debug Visualization Code
#collectConstraints(): void {
const fn = functionName(this.#collectConstraints);
const {
// vector3A-B used by setBabylonMatrixFromTransform()
vector3C: parentFramePositionBB,
vector3D: rotatedParentFramePositionBB,
vector3E: childFramePositionBB,
vector3F: rotatedChildFramePositionBB,
vector3G: fromLocalPositionBB,
vector3H: fromWorldPositionBB,
vector3I: toLocalPositionBB,
vector3J: toWorldPositionBB,
// quaternionA used by setBabylonMatrixFromTransform()
quaternionB: parentBodyRotationBB,
quaternionC: childBodyRotationBB,
quaternionD: parentFrameRotationBB,
quaternionE: childFrameRotationBB,
matrixA: parentBodyWorldMatrixBB,
matrixB: childBodyWorldMatrixBB,
matrixC: parentFrameWorldMatrixBB,
matrixD: childFrameWorldMatrixBB,
matrixE: parentWorldMatrixBB,
matrixF: childWorldMatrixBB,
matrixG: parentFrameRotationMatrixBB,
matrixH: childFrameRotationMatrixBB,
} = BABYLON_SHARED_DATA;
for (const constraint of this.#constraintMap.values()) {
// if (!constraint.inWorld) {
// continue;
// }
const id = constraint.vendorConstraint;
const {
parentBody,
childBody,
parentFrame,
childFrame,
} = constraint;
const [parentPosition, parentRotation] = parentBody.getTransform();
const [childPosition, childRotation] = childBody.getTransform();
setBabylonQuaternion(parentRotation, parentBodyRotationBB);
setBabylonQuaternion(childRotation, childBodyRotationBB);
setBabylonMatrixFromTransform(
{
position: parentPosition,
rotation: parentRotation,
},
parentBodyWorldMatrixBB,
);
setBabylonMatrixFromTransform(
{
position: childPosition,
rotation: childRotation,
},
childBodyWorldMatrixBB,
);
setBabylonVector3(parentFrame.position ?? ZERO_VECTOR3, parentFramePositionBB);
setBabylonQuaternion(parentFrame.rotation ?? IDENTITY_QUATERNION, parentFrameRotationBB);
setBabylonVector3(childFrame.position ?? ZERO_VECTOR3, childFramePositionBB);
setBabylonQuaternion(childFrame.rotation ?? IDENTITY_QUATERNION, childFrameRotationBB);
setBabylonMatrixFromTransform(parentFrame, parentFrameWorldMatrixBB);
setBabylonMatrixFromTransform(childFrame, childFrameWorldMatrixBB);
parentFrameWorldMatrixBB.multiplyToRef(parentBodyWorldMatrixBB, parentWorldMatrixBB);
childFrameWorldMatrixBB.multiplyToRef(childBodyWorldMatrixBB, childWorldMatrixBB);
if ((this.#debugDrawMode & DEBUG_DRAW_MODE.JOINT_AXIS) !== 0) {
this.#drawAxes(parentWorldMatrixBB);
this.#drawAxes(childWorldMatrixBB);
}
if ((this.#debugDrawMode & DEBUG_DRAW_MODE.JOINT_LIMIT) !== 0) {
const limitMode = runHPValueFunction(HK.HP_Constraint_GetAxisMode.bind(null, id, HK.ConstraintAxis.LINEAR_X));
if (limitMode === HK.ConstraintAxisLimitMode.LIMITED) {
const minLimit = runHPValueFunction(HK.HP_Constraint_GetAxisMinLimit.bind(null, id, HK.ConstraintAxis.LINEAR_X));
const maxLimit = runHPValueFunction(HK.HP_Constraint_GetAxisMaxLimit.bind(null, id, HK.ConstraintAxis.LINEAR_X));
setBabylonVector3(ZERO_VECTOR3, fromLocalPositionBB);
BABYLON.Vector3.TransformCoordinatesToRef(fromLocalPositionBB, parentWorldMatrixBB, fromWorldPositionBB);
setBabylonVector3(scaleVector3(X_AXIS_VECTOR3, maxLimit - minLimit), toLocalPositionBB);
BABYLON.Vector3.TransformCoordinatesToRef(toLocalPositionBB, parentWorldMatrixBB, toWorldPositionBB);
this._lines.push({
from: babylonVector3ToVector3(fromWorldPositionBB),
to: babylonVector3ToVector3(toWorldPositionBB),
fromColor: JOINT_LINEAR_LIMIT_COLOR,
toColor: JOINT_LINEAR_LIMIT_COLOR,
});
this.#drawCircle(parentWorldMatrixBB, HK.ConstraintAxis.LINEAR_X, fromLocalPositionBB, JOINT_LINEAR_LIMIT_RADIUS);
this.#drawCircle(parentWorldMatrixBB, HK.ConstraintAxis.LINEAR_X, toLocalPositionBB, JOINT_LINEAR_LIMIT_RADIUS);
}
let angularLimitMode = runHPValueFunction(HK.HP_Constraint_GetAxisMode.bind(null, id, HK.ConstraintAxis.ANGULAR_X));
if (angularLimitMode === HK.ConstraintAxisLimitMode.LIMITED) {
const minLimit = runHPValueFunction(HK.HP_Constraint_GetAxisMinLimit.bind(null, id, HK.ConstraintAxis.ANGULAR_X));
const maxLimit = runHPValueFunction(HK.HP_Constraint_GetAxisMaxLimit.bind(null, id, HK.ConstraintAxis.ANGULAR_X));
setBabylonVector3(ZERO_VECTOR3, fromLocalPositionBB);
BABYLON.Vector3.TransformCoordinatesToRef(fromLocalPositionBB, parentWorldMatrixBB, fromWorldPositionBB);
this.#drawArc(parentWorldMatrixBB, HK.ConstraintAxis.ANGULAR_X, fromLocalPositionBB, minLimit, maxLimit, JOINT_ANGULAR_LIMIT_RADIUS);
}
angularLimitMode = runHPValueFunction(HK.HP_Constraint_GetAxisMode.bind(null, id, HK.ConstraintAxis.ANGULAR_Y));
if (angularLimitMode === HK.ConstraintAxisLimitMode.LIMITED) {
const minLimit = runHPValueFunction(HK.HP_Constraint_GetAxisMinLimit.bind(null, id, HK.ConstraintAxis.ANGULAR_Y));
const maxLimit = runHPValueFunction(HK.HP_Constraint_GetAxisMaxLimit.bind(null, id, HK.ConstraintAxis.ANGULAR_Y));
setBabylonVector3(ZERO_VECTOR3, fromLocalPositionBB);
BABYLON.Vector3.TransformCoordinatesToRef(fromLocalPositionBB, parentWorldMatrixBB, fromWorldPositionBB);
this.#drawArc(parentWorldMatrixBB, HK.ConstraintAxis.ANGULAR_Y, fromLocalPositionBB, minLimit, maxLimit, JOINT_ANGULAR_LIMIT_RADIUS);
}
angularLimitMode = runHPValueFunction(HK.HP_Constraint_GetAxisMode.bind(null, id, HK.ConstraintAxis.ANGULAR_Z));
if (angularLimitMode === HK.ConstraintAxisLimitMode.LIMITED) {
const minLimit = runHPValueFunction(HK.HP_Constraint_GetAxisMinLimit.bind(null, id, HK.ConstraintAxis.ANGULAR_Z));
const maxLimit = runHPValueFunction(HK.HP_Constraint_GetAxisMaxLimit.bind(null, id, HK.ConstraintAxis.ANGULAR_Z));
setBabylonVector3(ZERO_VECTOR3, fromLocalPositionBB);
BABYLON.Vector3.TransformCoordinatesToRef(fromLocalPositionBB, parentWorldMatrixBB, fromWorldPositionBB);
this.#drawArc(parentWorldMatrixBB, HK.ConstraintAxis.ANGULAR_Z, fromLocalPositionBB, minLimit, maxLimit, JOINT_ANGULAR_LIMIT_RADIUS);
}
}
}
}
#drawAxes(frame: BABYLON.Matrix): void {
const {
// vector3A-B used by setBabylonMatrixFromTransform()
// vector3C-F used by #collectConstraints()
vector3G: fromLocalPositionBB,
vector3H: fromWorldPositionBB,
vector3I: toLocalPositionBB,
vector3J: toWorldPositionBB,
} = BABYLON_SHARED_DATA;
setBabylonVector3(ZERO_VECTOR3, fromLocalPositionBB);
BABYLON.Vector3.TransformCoordinatesToRef(fromLocalPositionBB, frame, fromWorldPositionBB);
setBabylonVector3(scaleVector3(X_AXIS_VECTOR3, JOINT_AXIS_FACTOR), toLocalPositionBB);
BABYLON.Vector3.TransformCoordinatesToRef(toLocalPositionBB, frame, toWorldPositionBB);
this._lines.push({
from: babylonVector3ToVector3(fromWorldPositionBB),
to: babylonVector3ToVector3(toWorldPositionBB),
fromColor: X_AXIS_COLOR,
toColor: X_AXIS_COLOR,
});
setBabylonVector3(scaleVector3(Y_AXIS_VECTOR3, JOINT_AXIS_FACTOR), toLocalPositionBB);
BABYLON.Vector3.TransformCoordinatesToRef(toLocalPositionBB, frame, toWorldPositionBB);
this._lines.push({
from: babylonVector3ToVector3(fromWorldPositionBB),
to: babylonVector3ToVector3(toWorldPositionBB),
fromColor: Y_AXIS_COLOR,
toColor: Y_AXIS_COLOR,
});
setBabylonVector3(scaleVector3(Z_AXIS_VECTOR3, JOINT_AXIS_FACTOR), toLocalPositionBB);
BABYLON.Vector3.TransformCoordinatesToRef(toLocalPositionBB, frame, toWorldPositionBB);
this._lines.push({
from: babylonVector3ToVector3(fromWorldPositionBB),
to: babylonVector3ToVector3(toWorldPositionBB),
fromColor: Z_AXIS_COLOR,
toColor: Z_AXIS_COLOR,
});
}
#drawCircle(frame: BABYLON.Matrix, axis: ConstraintAxis, center: BABYLON.Vector3, radius: number): void {
const fn = functionName(this.#drawCircle);
const p1 = center.clone();
const p2 = center.clone();
const p3 = center.clone();
switch (axis) {
case HK.ConstraintAxis.LINEAR_X:
p1.z -= radius;
p2.y += radius;
p3.z += radius;
break;
case HK.ConstraintAxis.LINEAR_Y:
p1.x -= radius;
p2.z += radius;
p3.x += radius;
break;
case HK.ConstraintAxis.LINEAR_Z:
p1.y -= radius;
p2.x += radius;
p3.y += radius;
break;
default:
threadLogger().fatal(`${fn}default: axis: ${axis}`);
}
BABYLON.Vector3.TransformCoordinatesToRef(p1, frame, p1);
BABYLON.Vector3.TransformCoordinatesToRef(p2, frame, p2);
BABYLON.Vector3.TransformCoordinatesToRef(p3, frame, p3);
const arc = BABYLON.Curve3.ArcThru3Points(p1, p2, p3, STEPS_PER_CIRCLE, false, true);
const points = arc.getPoints();
let prevPoint: BABYLON.Vector3 | undefined;
for (let i = 0; i < points.length; ++i) {
const point = points[i]!;
if (prevPoint !== undefined) {
this._lines.push({
from: babylonVector3ToVector3(prevPoint),
to: babylonVector3ToVector3(point),
fromColor: JOINT_LINEAR_LIMIT_COLOR,
toColor: JOINT_LINEAR_LIMIT_COLOR,
});
}
prevPoint = point;
}
}
#drawArc(
//
frame: BABYLON.Matrix,
axis: ConstraintAxis,
center: BABYLON.Vector3,
minAngle: number,
maxAngle: number,
radius: number,
): void {
const fn = functionName(this.#drawArc);
center = center.clone();
const p1 = center.clone();
const p2 = center.clone();
const p3 = center.clone();
switch (axis) {
case HK.ConstraintAxis.ANGULAR_X:
minAngle += Math.PI / 2;
maxAngle += Math.PI / 2;
p1.z -= Math.cos(minAngle) * radius;
p1.y += Math.sin(minAngle) * radius;
p2.z -= Math.cos((minAngle + maxAngle) / 2) * radius;
p2.y += Math.sin((minAngle + maxAngle) / 2) * radius;
p3.z -= Math.cos(maxAngle) * radius;
p3.y += Math.sin(maxAngle) * radius;
break;
case HK.ConstraintAxis.ANGULAR_Y:
p1.x += Math.cos(minAngle) * radius;
p1.z -= Math.sin(minAngle) * radius;
p2.x += Math.cos((minAngle + maxAngle) / 2) * radius;
p2.z -= Math.sin((minAngle + maxAngle) / 2) * radius;
p3.x += Math.cos(maxAngle) * radius;
p3.z -= Math.sin(maxAngle) * radius;
break;
case HK.ConstraintAxis.ANGULAR_Z:
p1.x += Math.cos(minAngle) * radius;
p1.y += Math.sin(minAngle) * radius;
p2.x += Math.cos((minAngle + maxAngle) / 2) * radius;
p2.y += Math.sin((minAngle + maxAngle) / 2) * radius;
p3.x += Math.cos(maxAngle) * radius;
p3.y += Math.sin(maxAngle) * radius;
break;
default:
threadLogger().fatal(`${fn}default: axis: ${axis}`);
}
BABYLON.Vector3.TransformCoordinatesToRef(center, frame, center);
BABYLON.Vector3.TransformCoordinatesToRef(p1, frame, p1);
BABYLON.Vector3.TransformCoordinatesToRef(p2, frame, p2);
BABYLON.Vector3.TransformCoordinatesToRef(p3, frame, p3);
this._points.push({
position: babylonVector3ToVector3(p1),
color: X_AXIS_COLOR,
});
this._points.push({
position: babylonVector3ToVector3(p2),
color: Y_AXIS_COLOR,
});
this._points.push({
position: babylonVector3ToVector3(p3),
color: Z_AXIS_COLOR,
});
const stepFactor = Math.abs(maxAngle - minAngle) / (2 * Math.PI);
const numSteps = Math.floor(stepFactor * STEPS_PER_CIRCLE);
// threadLogger().verbose(`${fn}stepFactor: ${stepFactor}, numSteps: ${numSteps}`);
const arc = BABYLON.Curve3.ArcThru3Points(p1, p2, p3, numSteps);
const points = [center, ...arc.getPoints(), center];
let prevPoint: BABYLON.Vector3 | undefined;
for (let i = 0; i < points.length; ++i) {
const point = points[i]!;
if (prevPoint !== undefined) {
this._lines.push({
from: babylonVector3ToVector3(prevPoint),
to: babylonVector3ToVector3(point),
fromColor: JOINT_ANGULAR_LIMIT_COLOR,
toColor: JOINT_ANGULAR_LIMIT_COLOR,
});
}
prevPoint = point;
}
}
A. limitation with the code above is that the distance limits (yellow circles) are currently only drawn for the x-axis, so code needs to be added for the y- and z-axes
I love the list of features you shared.
If it can help, I can take a video of built-in debug drawings for different joint types in Bullet and PhysX. We could use good aspects from both in the Havok joint debug drawing
