chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Set tolerance in GJK to zero for colliding discrete geoms.

Browse Source 
PiperOrigin-RevId: 672326091
Change-Id: Iddec335272461c9aee13c1960640a453ff59f841
This commit is contained in:
Kyle Bayes
2024-09-08 13:48:55 -07:00
committed by Copybara-Service
parent 56bb3ca5de
commit 61721f8d3c
3 changed files with 72 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_util_blas.c
+39 -30
View File
@@ -38,6 +38,15 @@ void mju_zero3(mjtNum res[3]) {
// vec1 == vec2
int mju_equal3(const mjtNum vec1[3], const mjtNum vec2[3]) {
return mju_abs(vec1[0] - vec2[0]) < mjMINVAL &&
mju_abs(vec1[1] - vec2[1]) < mjMINVAL &&
mju_abs(vec1[2] - vec2[2]) < mjMINVAL;
}
// res = vec
void mju_copy3(mjtNum res[3], const mjtNum data[3]) {
res[0] = data[0];
@@ -195,46 +204,46 @@ void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) {
// multiply 3x3 matrices,
void mju_mulMatMat3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]) {
res[0] = a[0]*b[0] + a[1]*b[3] + a[2]*b[6];
res[1] = a[0]*b[1] + a[1]*b[4] + a[2]*b[7];
res[2] = a[0]*b[2] + a[1]*b[5] + a[2]*b[8];
res[3] = a[3]*b[0] + a[4]*b[3] + a[5]*b[6];
res[4] = a[3]*b[1] + a[4]*b[4] + a[5]*b[7];
res[5] = a[3]*b[2] + a[4]*b[5] + a[5]*b[8];
res[6] = a[6]*b[0] + a[7]*b[3] + a[8]*b[6];
res[7] = a[6]*b[1] + a[7]*b[4] + a[8]*b[7];
res[8] = a[6]*b[2] + a[7]*b[5] + a[8]*b[8];
void mju_mulMatMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) {
res[0] = mat1[0]*mat2[0] + mat1[1]*mat2[3] + mat1[2]*mat2[6];
res[1] = mat1[0]*mat2[1] + mat1[1]*mat2[4] + mat1[2]*mat2[7];
res[2] = mat1[0]*mat2[2] + mat1[1]*mat2[5] + mat1[2]*mat2[8];
res[3] = mat1[3]*mat2[0] + mat1[4]*mat2[3] + mat1[5]*mat2[6];
res[4] = mat1[3]*mat2[1] + mat1[4]*mat2[4] + mat1[5]*mat2[7];
res[5] = mat1[3]*mat2[2] + mat1[4]*mat2[5] + mat1[5]*mat2[8];
res[6] = mat1[6]*mat2[0] + mat1[7]*mat2[3] + mat1[8]*mat2[6];
res[7] = mat1[6]*mat2[1] + mat1[7]*mat2[4] + mat1[8]*mat2[7];
res[8] = mat1[6]*mat2[2] + mat1[7]*mat2[5] + mat1[8]*mat2[8];
}
// multiply 3x3 matrices, first argument transposed
void mju_mulMatTMat3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]) {
res[0] = a[0]*b[0] + a[3]*b[3] + a[6]*b[6];
res[1] = a[0]*b[1] + a[3]*b[4] + a[6]*b[7];
res[2] = a[0]*b[2] + a[3]*b[5] + a[6]*b[8];
res[3] = a[1]*b[0] + a[4]*b[3] + a[7]*b[6];
res[4] = a[1]*b[1] + a[4]*b[4] + a[7]*b[7];
res[5] = a[1]*b[2] + a[4]*b[5] + a[7]*b[8];
res[6] = a[2]*b[0] + a[5]*b[3] + a[8]*b[6];
res[7] = a[2]*b[1] + a[5]*b[4] + a[8]*b[7];
res[8] = a[2]*b[2] + a[5]*b[5] + a[8]*b[8];
void mju_mulMatTMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) {
res[0] = mat1[0]*mat2[0] + mat1[3]*mat2[3] + mat1[6]*mat2[6];
res[1] = mat1[0]*mat2[1] + mat1[3]*mat2[4] + mat1[6]*mat2[7];
res[2] = mat1[0]*mat2[2] + mat1[3]*mat2[5] + mat1[6]*mat2[8];
res[3] = mat1[1]*mat2[0] + mat1[4]*mat2[3] + mat1[7]*mat2[6];
res[4] = mat1[1]*mat2[1] + mat1[4]*mat2[4] + mat1[7]*mat2[7];
res[5] = mat1[1]*mat2[2] + mat1[4]*mat2[5] + mat1[7]*mat2[8];
res[6] = mat1[2]*mat2[0] + mat1[5]*mat2[3] + mat1[8]*mat2[6];
res[7] = mat1[2]*mat2[1] + mat1[5]*mat2[4] + mat1[8]*mat2[7];
res[8] = mat1[2]*mat2[2] + mat1[5]*mat2[5] + mat1[8]*mat2[8];
}
// multiply 3x3 matrices, second argument transposed
void mju_mulMatMatT3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]) {
res[0] = a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
res[1] = a[0]*b[3] + a[1]*b[4] + a[2]*b[5];
res[2] = a[0]*b[6] + a[1]*b[7] + a[2]*b[8];
res[3] = a[3]*b[0] + a[4]*b[1] + a[5]*b[2];
res[4] = a[3]*b[3] + a[4]*b[4] + a[5]*b[5];
res[5] = a[3]*b[6] + a[4]*b[7] + a[5]*b[8];
res[6] = a[6]*b[0] + a[7]*b[1] + a[8]*b[2];
res[7] = a[6]*b[3] + a[7]*b[4] + a[8]*b[5];
res[8] = a[6]*b[6] + a[7]*b[7] + a[8]*b[8];
void mju_mulMatMatT3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) {
res[0] = mat1[0]*mat2[0] + mat1[1]*mat2[1] + mat1[2]*mat2[2];
res[1] = mat1[0]*mat2[3] + mat1[1]*mat2[4] + mat1[2]*mat2[5];
res[2] = mat1[0]*mat2[6] + mat1[1]*mat2[7] + mat1[2]*mat2[8];
res[3] = mat1[3]*mat2[0] + mat1[4]*mat2[1] + mat1[5]*mat2[2];
res[4] = mat1[3]*mat2[3] + mat1[4]*mat2[4] + mat1[5]*mat2[5];
res[5] = mat1[3]*mat2[6] + mat1[4]*mat2[7] + mat1[5]*mat2[8];
res[6] = mat1[6]*mat2[0] + mat1[7]*mat2[1] + mat1[8]*mat2[2];
res[7] = mat1[6]*mat2[3] + mat1[7]*mat2[4] + mat1[8]*mat2[5];
res[8] = mat1[6]*mat2[6] + mat1[7]*mat2[7] + mat1[8]*mat2[8];
}