nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Implement revolute narrow

Browse Source
This commit is contained in:
Emil Ernerfeldt
2021-02-17 20:22:41 +01:00
parent d3f39a9bab
commit 48708d9a76
2 changed files with 70 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
View File

@@ -155,14 +155,14 @@ impl PrismaticVelocityConstraint {
let limit_err = dpos.dot(&axis1);
let mut linear_err = dpos - *axis1 * limit_err;
let frame1 = rb1.position * cparams.local_frame1();
let frame2 = rb2.position * cparams.local_frame2();
let frame1 = rb1.position * joint.local_frame1();
let frame2 = rb2.position * joint.local_frame2();
let ang_err = frame2.rotation * frame1.rotation.inverse();
if limit_err < cparams.limits[0] {
linear_err += *axis1 * (limit_err - cparams.limits[0]);
} else if limit_err > cparams.limits[1] {
linear_err += *axis1 * (limit_err - cparams.limits[1]);
if limit_err < joint.limits[0] {
linear_err += *axis1 * (limit_err - joint.limits[0]);
} else if limit_err > joint.limits[1] {
linear_err += *axis1 * (limit_err - joint.limits[1]);
}
#[cfg(feature = "dim2")]
@@ -572,11 +572,11 @@ impl PrismaticVelocityGroundConstraint {
if velocity_based_erp_inv_dt != 0.0 {
let (frame1, frame2);
if flipped {
frame1 = rb1.position * cparams.local_frame2();
frame2 = rb2.position * cparams.local_frame1();
frame1 = rb1.position * joint.local_frame2();
frame2 = rb2.position * joint.local_frame1();
} else {
frame1 = rb1.position * cparams.local_frame1();
frame2 = rb2.position * cparams.local_frame2();
frame1 = rb1.position * joint.local_frame1();
frame2 = rb2.position * joint.local_frame2();
}
let dpos = anchor2 - anchor1;
@@ -585,10 +585,10 @@ impl PrismaticVelocityGroundConstraint {
let ang_err = frame2.rotation * frame1.rotation.inverse();
if limit_err < cparams.limits[0] {
linear_err += *axis1 * (limit_err - cparams.limits[0]);
} else if limit_err > cparams.limits[1] {
linear_err += *axis1 * (limit_err - cparams.limits[1]);
if limit_err < joint.limits[0] {
linear_err += *axis1 * (limit_err - joint.limits[0]);
} else if limit_err > joint.limits[1] {
linear_err += *axis1 * (limit_err - joint.limits[1]);
}
#[cfg(feature = "dim2")]

64
src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
View File

@@ -3,9 +3,8 @@ use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
};
use crate::math::{AngularInertia, Real, Rotation, Vector};
use crate::na::UnitQuaternion;
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
use na::{Cholesky, Matrix3x2, Matrix5, UnitQuaternion, Vector5, U2, U3};
#[derive(Debug)]
pub(crate) struct RevoluteVelocityConstraint {
@@ -90,9 +89,31 @@ impl RevoluteVelocityConstraint {
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
let linvel_err =
(rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
let mut rhs = Vector5::new(
linvel_err.x,
linvel_err.y,
linvel_err.z,
angvel_err.x,
angvel_err.y,
) * params.velocity_solve_fraction;
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
if velocity_based_erp_inv_dt != 0.0 {
let lin_err = anchor2 - anchor1;
let axis1 = rb1.position * joint.local_axis1;
let axis2 = rb2.position * joint.local_axis2;
let ang_err =
Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity);
let ang_err = ang_err.scaled_axis();
rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
* velocity_based_erp_inv_dt;
}
/*
* Motor.
@@ -371,9 +392,36 @@ impl RevoluteVelocityGroundConstraint {
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
let linvel_err =
(rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
let mut rhs = Vector5::new(
linvel_err.x,
linvel_err.y,
linvel_err.z,
angvel_err.x,
angvel_err.y,
) * params.velocity_solve_fraction;
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
if velocity_based_erp_inv_dt != 0.0 {
let lin_err = anchor2 - anchor1;
let (axis1, axis2);
if flipped {
axis1 = rb1.position * joint.local_axis2;
axis2 = rb2.position * joint.local_axis1;
} else {
axis1 = rb1.position * joint.local_axis1;
axis2 = rb2.position * joint.local_axis2;
}
let ang_err =
Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity);
let ang_err = ang_err.scaled_axis();
rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
* velocity_based_erp_inv_dt;
}
/*
* Motor part.