diff --git a/src.ts/dynamics/multibody_joint.ts b/src.ts/dynamics/multibody_joint.ts index 146caaad..5e55f870 100644 --- a/src.ts/dynamics/multibody_joint.ts +++ b/src.ts/dynamics/multibody_joint.ts @@ -1,16 +1,24 @@ -import {RawImpulseJointSet, RawJointAxis, RawMultibodyJointSet} from "../raw"; +import {Rotation, Vector, VectorOps, RotationOps} from "../math"; +import { + RawGenericJoint, + RawMultibodyJointSet, + RawRigidBodySet, + RawJointAxis, +} from "../raw"; + import { FixedImpulseJoint, - ImpulseJointHandle, + // MultibodyJointHandle, JointType, MotorModel, PrismaticImpulseJoint, RevoluteImpulseJoint, } from "./impulse_joint"; +import {RigidBody, RigidBodyHandle} from "./rigid_body"; +import {RigidBodySet} from "./rigid_body_set"; // #if DIM3 import {Quaternion} from "../math"; -import {SphericalImpulseJoint} from "./impulse_joint"; // #endif /** @@ -20,15 +28,22 @@ export type MultibodyJointHandle = number; export class MultibodyJoint { protected rawSet: RawMultibodyJointSet; // The MultibodyJoint won't need to free this. + // protected bodySet: RigidBodySet; // The MultibodyJoint won’t need to free this. handle: MultibodyJointHandle; - constructor(rawSet: RawMultibodyJointSet, handle: MultibodyJointHandle) { + constructor( + rawSet: RawMultibodyJointSet, + // bodySet: RigidBodySet, + handle: MultibodyJointHandle, + ) { this.rawSet = rawSet; + // this.bodySet = bodySet; this.handle = handle; } public static newTyped( rawSet: RawMultibodyJointSet, + // bodySet: RigidBodySet, handle: MultibodyJointHandle, ): MultibodyJoint { switch (rawSet.jointType(handle)) { @@ -47,6 +62,11 @@ export class MultibodyJoint { } } + /** @internal */ + public finalizeDeserialization(bodySet: RigidBodySet) { + // this.bodySet = bodySet; + } + /** * Checks if this joint is still valid (i.e. that it has * not been deleted from the joint set yet). @@ -56,66 +76,90 @@ export class MultibodyJoint { } // /** - // * The unique integer identifier of the first rigid-body this joint it attached to. - // */ - // public bodyHandle1(): RigidBodyHandle { - // return this.rawSet.jointBodyHandle1(this.handle); - // } - // - // /** - // * The unique integer identifier of the second rigid-body this joint is attached to. - // */ - // public bodyHandle2(): RigidBodyHandle { - // return this.rawSet.jointBodyHandle2(this.handle); - // } - // - // /** - // * The type of this joint given as a string. + // * The first rigid-body this joint it attached to. // */ - // public type(): JointType { - // return this.rawSet.jointType(this.handle); + // public body1(): RigidBody { + // return this.bodySet.get(this.rawSet.jointBodyHandle1(this.handle)); // } // - // // #if DIM3 // /** - // * The rotation quaternion that aligns this joint's first local axis to the `x` axis. + // * The second rigid-body this joint is attached to. // */ - // public frameX1(): Rotation { - // return RotationOps.fromRaw(this.rawSet.jointFrameX1(this.handle)); - // } - // - // // #endif - // - // // #if DIM3 - // /** - // * The rotation matrix that aligns this joint's second local axis to the `x` axis. - // */ - // public frameX2(): Rotation { - // return RotationOps.fromRaw(this.rawSet.jointFrameX2(this.handle)); - // } - // - // // #endif - // - // /** - // * The position of the first anchor of this joint. - // * - // * The first anchor gives the position of the points application point on the - // * local frame of the first rigid-body it is attached to. - // */ - // public anchor1(): Vector { - // return VectorOps.fromRaw(this.rawSet.jointAnchor1(this.handle)); - // } - // - // /** - // * The position of the second anchor of this joint. - // * - // * The second anchor gives the position of the points application point on the - // * local frame of the second rigid-body it is attached to. - // */ - // public anchor2(): Vector { - // return VectorOps.fromRaw(this.rawSet.jointAnchor2(this.handle)); + // public body2(): RigidBody { + // return this.bodySet.get(this.rawSet.jointBodyHandle2(this.handle)); // } + /** + * The type of this joint given as a string. + */ + public type(): JointType { + return this.rawSet.jointType(this.handle); + } + + // #if DIM3 + /** + * The rotation quaternion that aligns this joint's first local axis to the `x` axis. + */ + public frameX1(): Rotation { + return RotationOps.fromRaw(this.rawSet.jointFrameX1(this.handle)); + } + + // #endif + + // #if DIM3 + /** + * The rotation matrix that aligns this joint's second local axis to the `x` axis. + */ + public frameX2(): Rotation { + return RotationOps.fromRaw(this.rawSet.jointFrameX2(this.handle)); + } + + // #endif + + /** + * The position of the first anchor of this joint. + * + * The first anchor gives the position of the application point on the + * local frame of the first rigid-body it is attached to. + */ + public anchor1(): Vector { + return VectorOps.fromRaw(this.rawSet.jointAnchor1(this.handle)); + } + + /** + * The position of the second anchor of this joint. + * + * The second anchor gives the position of the application point on the + * local frame of the second rigid-body it is attached to. + */ + public anchor2(): Vector { + return VectorOps.fromRaw(this.rawSet.jointAnchor2(this.handle)); + } + + /** + * Sets the position of the first anchor of this joint. + * + * The first anchor gives the position of the application point on the + * local frame of the first rigid-body it is attached to. + */ + public setAnchor1(newPos: Vector) { + const rawPoint = VectorOps.intoRaw(newPos); + this.rawSet.jointSetAnchor1(this.handle, rawPoint); + rawPoint.free(); + } + + /** + * Sets the position of the second anchor of this joint. + * + * The second anchor gives the position of the application point on the + * local frame of the second rigid-body it is attached to. + */ + public setAnchor2(newPos: Vector) { + const rawPoint = VectorOps.intoRaw(newPos); + this.rawSet.jointSetAnchor2(this.handle, rawPoint); + rawPoint.free(); + } + /** * Controls whether contacts are computed between colliders attached * to the rigid-bodies linked by this joint. @@ -139,42 +183,83 @@ export class UnitMultibodyJoint extends MultibodyJoint { */ protected rawAxis?(): RawJointAxis; - // /** - // * Are the limits enabled for this joint? - // */ - // public limitsEnabled(): boolean { - // return this.rawSet.jointLimitsEnabled(this.handle, this.rawAxis()); - // } - // - // /** - // * The min limit of this joint. - // */ - // public limitsMin(): number { - // return this.rawSet.jointLimitsMin(this.handle, this.rawAxis()); - // } - // - // /** - // * The max limit of this joint. - // */ - // public limitsMax(): number { - // return this.rawSet.jointLimitsMax(this.handle, this.rawAxis()); - // } - // - // public configureMotorModel(model: MotorModel) { - // this.rawSet.jointConfigureMotorModel(this.handle, this.rawAxis(), model); - // } - // - // public configureMotorVelocity(targetVel: number, factor: number) { - // this.rawSet.jointConfigureMotorVelocity(this.handle, this.rawAxis(), targetVel, factor); - // } - // - // public configureMotorPosition(targetPos: number, stiffness: number, damping: number) { - // this.rawSet.jointConfigureMotorPosition(this.handle, this.rawAxis(), targetPos, stiffness, damping); - // } - // - // public configureMotor(targetPos: number, targetVel: number, stiffness: number, damping: number) { - // this.rawSet.jointConfigureMotor(this.handle, this.rawAxis(), targetPos, targetVel, stiffness, damping); - // } + /** + * Are the limits enabled for this joint? + */ + public limitsEnabled(): boolean { + return this.rawSet.jointLimitsEnabled(this.handle, this.rawAxis()); + } + + /** + * The min limit of this joint. + */ + public limitsMin(): number { + return this.rawSet.jointLimitsMin(this.handle, this.rawAxis()); + } + + /** + * The max limit of this joint. + */ + public limitsMax(): number { + return this.rawSet.jointLimitsMax(this.handle, this.rawAxis()); + } + + /** + * Sets the limits of this joint. + * + * @param min - The minimum bound of this joint’s free coordinate. + * @param max - The maximum bound of this joint’s free coordinate. + */ + public setLimits(min: number, max: number) { + this.rawSet.jointSetLimits(this.handle, this.rawAxis(), min, max); + } + + public configureMotorModel(model: MotorModel) { + this.rawSet.jointConfigureMotorModel( + this.handle, + this.rawAxis(), + model, + ); + } + + public configureMotorVelocity(targetVel: number, factor: number) { + this.rawSet.jointConfigureMotorVelocity( + this.handle, + this.rawAxis(), + targetVel, + factor, + ); + } + + public configureMotorPosition( + targetPos: number, + stiffness: number, + damping: number, + ) { + this.rawSet.jointConfigureMotorPosition( + this.handle, + this.rawAxis(), + targetPos, + stiffness, + damping, + ); + } + + public configureMotor( + targetPos: number, + targetVel: number, + stiffness: number, + damping: number, + ) { + this.rawSet.jointConfigureMotor( + this.handle, + this.rawAxis(), + targetPos, + targetVel, + stiffness, + damping, + ); + } } export class FixedMultibodyJoint extends MultibodyJoint {} diff --git a/src.ts/dynamics/multibody_joint_set.ts b/src.ts/dynamics/multibody_joint_set.ts index 2fbb3403..159b4bc1 100644 --- a/src.ts/dynamics/multibody_joint_set.ts +++ b/src.ts/dynamics/multibody_joint_set.ts @@ -2,10 +2,12 @@ import {RawMultibodyJointSet} from "../raw"; import {Coarena} from "../coarena"; import {RigidBodySet} from "./rigid_body_set"; import { - MultibodyJoint, - MultibodyJointHandle, RevoluteMultibodyJoint, FixedMultibodyJoint, + MultibodyJoint, + MultibodyJointHandle, + // JointData, + // JointType, PrismaticMultibodyJoint, // #if DIM3 SphericalMultibodyJoint, @@ -13,8 +15,8 @@ import { } from "./multibody_joint"; import {ImpulseJointHandle, JointData, JointType} from "./impulse_joint"; import {IslandManager} from "./island_manager"; -import {ColliderHandle} from "../geometry"; import {RigidBodyHandle} from "./rigid_body"; +import {Collider, ColliderHandle} from "../geometry"; /** * A set of joints. @@ -47,14 +49,20 @@ export class MultibodyJointSet { // Initialize the map with the existing elements, if any. if (raw) { raw.forEachJointHandle((handle: MultibodyJointHandle) => { - this.map.set(handle, MultibodyJoint.newTyped(this.raw, handle)); + this.map.set(handle, MultibodyJoint.newTyped(raw, handle)); }); } } + /** @internal */ + public finalizeDeserialization(bodies: RigidBodySet) { + this.map.forEach((joint) => joint.finalizeDeserialization(bodies)); + } + /** * Creates a new joint and return its integer handle. * + * @param bodies - The set of rigid-bodies containing the bodies the joint is attached to. * @param desc - The joint's parameters. * @param parent1 - The handle of the first rigid-body this joint is attached to. * @param parent2 - The handle of the second rigid-body this joint is attached to. @@ -83,11 +91,23 @@ export class MultibodyJointSet { * Remove a joint from this set. * * @param handle - The integer handle of the joint. - * @param wake_up - If `true`, the rigid-bodies attached by the removed joint will be woken-up automatically. + * @param wakeUp - If `true`, the rigid-bodies attached by the removed joint will be woken-up automatically. */ - public remove(handle: MultibodyJointHandle, wake_up: boolean) { - this.raw.remove(handle, wake_up); - this.map.delete(handle); + public remove(handle: MultibodyJointHandle, wakeUp: boolean) { + this.raw.remove(handle, wakeUp); + this.unmap(handle); + } + + /** + * Calls the given closure with the integer handle of each multibody joint attached to this rigid-body. + * + * @param f - The closure called with the integer handle of each multibody joint attached to the rigid-body. + */ + public forEachJointHandleAttachedToRigidBody( + handle: RigidBodyHandle, + f: (handle: MultibodyJointHandle) => void, + ) { + this.raw.forEachJointAttachedToRigidBody(handle, f); } /** @@ -134,18 +154,6 @@ export class MultibodyJointSet { this.map.forEach(f); } - /** - * Calls the given closure with the integer handle of each multibody joint attached to this rigid-body. - * - * @param f - The closure called with the integer handle of each multibody joint attached to the rigid-body. - */ - public forEachJointHandleAttachedToRigidBody( - handle: RigidBodyHandle, - f: (handle: MultibodyJointHandle) => void, - ) { - this.raw.forEachJointAttachedToRigidBody(handle, f); - } - /** * Gets all joints in the list. * diff --git a/src/dynamics/multibody_joint.rs b/src/dynamics/multibody_joint.rs index ff6e816e..18fb9516 100644 --- a/src/dynamics/multibody_joint.rs +++ b/src/dynamics/multibody_joint.rs @@ -1,6 +1,6 @@ -use crate::dynamics::{RawJointAxis, RawJointType, RawMultibodyJointSet}; +use crate::dynamics::{RawMultibodyJointSet, RawJointAxis, RawJointType, RawMotorModel}; use crate::math::{RawRotation, RawVector}; -use crate::utils::FlatHandle; +use crate::utils::{self, FlatHandle}; use rapier::dynamics::JointAxis; use wasm_bindgen::prelude::*; @@ -11,14 +11,14 @@ impl RawMultibodyJointSet { self.map(handle, |j| j.data.locked_axes.into()) } - // /// The unique integer identifier of the first rigid-body this joint it attached to. - // pub fn jointBodyHandle1(&self, handle: FlatHandle) -> u32 { - // self.map(handle, |j| j.body1.into_raw_parts().0) + /// The unique integer identifier of the first rigid-body this joint it attached to. + // pub fn jointBodyHandle1(&self, handle: FlatHandle) -> FlatHandle { + // self.map(handle, |j| utils::flat_handle(j.body1.0)) // } - // - // /// The unique integer identifier of the second rigid-body this joint is attached to. - // pub fn jointBodyHandle2(&self, handle: FlatHandle) -> u32 { - // self.map(handle, |j| j.body2.into_raw_parts().0) + + /// The unique integer identifier of the second rigid-body this joint is attached to. + // pub fn jointBodyHandle2(&self, handle: FlatHandle) -> FlatHandle { + // self.map(handle, |j| utils::flat_handle(j.body2.0)) // } /// The angular part of the joint’s local frame relative to the first rigid-body it is attached to. @@ -47,6 +47,20 @@ impl RawMultibodyJointSet { self.map(handle, |j| j.data.local_frame2.translation.vector.into()) } + /// Sets the position of the first local anchor + pub fn jointSetAnchor1(&mut self, handle: FlatHandle, newPos: &RawVector) { + self.map_mut(handle, |j| { + j.data.set_local_anchor1(newPos.0.into()); + }); + } + + /// Sets the position of the second local anchor + pub fn jointSetAnchor2(&mut self, handle: FlatHandle, newPos: &RawVector) { + self.map_mut(handle, |j| { + j.data.set_local_anchor2(newPos.0.into()); + }) + } + /// Are contacts between the rigid-bodies attached by this joint enabled? pub fn jointContactsEnabled(&self, handle: FlatHandle) -> bool { self.map(handle, |j| j.data.contacts_enabled) @@ -76,121 +90,57 @@ impl RawMultibodyJointSet { self.map(handle, |j| j.data.limits[axis as usize].max) } - // pub fn jointConfigureMotorModel( - // &mut self, - // handle: FlatHandle, - // axis: RawJointAxis, - // model: RawMotorModel, - // ) { - // self.map_mut(handle, |j| { - // j.data.motors[axis as usize].model = model.into() - // }) - // } + /// Enables and sets the joint limits + pub fn jointSetLimits(&mut self, handle: FlatHandle, axis: RawJointAxis, min: f32, max: f32) { + self.map_mut(handle, |j| { + j.data.set_limits(axis.into(), [min, max]); + }); + } - /* - #[cfg(feature = "dim3")] - pub fn jointConfigureBallMotorVelocity( + pub fn jointConfigureMotorModel( &mut self, handle: FlatHandle, - vx: f32, - vy: f32, - vz: f32, - factor: f32, + axis: RawJointAxis, + model: RawMotorModel, ) { - let targetVel = Vector3::new(vx, vy, vz); - - self.map_mut(handle, |j| match &mut j.params { - JointData::SphericalJoint(j) => j.configure_motor_velocity(targetVel, factor), - _ => {} + self.map_mut(handle, |j| { + j.data.motors[axis as usize].model = model.into() }) } - #[cfg(feature = "dim3")] - pub fn jointConfigureBallMotorPosition( + pub fn jointConfigureMotorVelocity( + &mut self, + handle: FlatHandle, + axis: RawJointAxis, + targetVel: f32, + factor: f32, + ) { + self.jointConfigureMotor(handle, axis, 0.0, targetVel, 0.0, factor) + } + + pub fn jointConfigureMotorPosition( &mut self, handle: FlatHandle, - qw: f32, - qx: f32, - qy: f32, - qz: f32, + axis: RawJointAxis, + targetPos: f32, stiffness: f32, damping: f32, ) { - let quat = Quaternion::new(qw, qx, qy, qz); - - self.map_mut(handle, |j| match &mut j.params { - JointData::SphericalJoint(j) => { - if let Some(unit_quat) = UnitQuaternion::try_new(quat, 1.0e-5) { - j.configure_motor_position(unit_quat, stiffness, damping) - } - } - _ => {} - }) + self.jointConfigureMotor(handle, axis, targetPos, 0.0, stiffness, damping) } - #[cfg(feature = "dim3")] - pub fn jointConfigureBallMotor( + pub fn jointConfigureMotor( &mut self, handle: FlatHandle, - qw: f32, - qx: f32, - qy: f32, - qz: f32, - vx: f32, - vy: f32, - vz: f32, + axis: RawJointAxis, + targetPos: f32, + targetVel: f32, stiffness: f32, damping: f32, ) { - let quat = Quaternion::new(qw, qx, qy, qz); - let vel = Vector3::new(vx, vy, vz); - - self.map_mut(handle, |j| match &mut j.params { - JointData::SphericalJoint(j) => { - if let Some(unit_quat) = UnitQuaternion::try_new(quat, 1.0e-5) { - j.configure_motor(unit_quat, vel, stiffness, damping) - } - } - _ => {} + self.map_mut(handle, |j| { + j.data + .set_motor(axis.into(), targetPos, targetVel, stiffness, damping); }) } - */ - - // pub fn jointConfigureMotorVelocity( - // &mut self, - // handle: FlatHandle, - // axis: RawJointAxis, - // targetVel: f32, - // factor: f32, - // ) { - // self.jointConfigureMotor(handle, axis, 0.0, targetVel, 0.0, factor) - // } - // - // pub fn jointConfigureMotorPosition( - // &mut self, - // handle: FlatHandle, - // axis: RawJointAxis, - // targetPos: f32, - // stiffness: f32, - // damping: f32, - // ) { - // self.jointConfigureMotor(handle, axis, targetPos, 0.0, stiffness, damping) - // } - - // pub fn jointConfigureMotor( - // &mut self, - // handle: FlatHandle, - // axis: RawJointAxis, - // targetPos: f32, - // targetVel: f32, - // stiffness: f32, - // damping: f32, - // ) { - // self.map_mut(handle, |j| { - // j.data.motors[axis as usize].target_pos = targetPos; - // j.data.motors[axis as usize].target_vel = targetVel; - // j.data.motors[axis as usize].stiffness = stiffness; - // j.data.motors[axis as usize].damping = damping; - // }) - // } }