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

Cosmetic clean-up and a few micro-optimisations for primitive colliders.

Browse Source 
PiperOrigin-RevId: 543559919
Change-Id: I345ccbdca4303e82d1097a5a4bf9381c9f845617
This commit is contained in:
Yuval Tassa
2023-06-26 15:35:40 -07:00
committed by Copybara-Service
parent c50c92ccc8
commit 1d6d6f33bf
1 changed files with 68 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_primitive.c
+68 -118
View File
@@ -20,6 +20,7 @@
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include "engine/engine_util_blas.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_spatial.h"
@@ -27,19 +28,16 @@
// plane : sphere (actual implementation, can be called with modified parameters)
static int _PlaneSphere(mjContact* con, mjtNum margin,
mjtNum* pos1, mjtNum* mat1, mjtNum* size1,
mjtNum* pos2, mjtNum* mat2, mjtNum* size2) {
mjtNum tmp[3];
mjtNum cdist;
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
// set normal
con[0].frame[0] = mat1[2];
con[0].frame[1] = mat1[5];
con[0].frame[2] = mat1[8];
// compute distance, return if too large
mju_sub3(tmp, pos2, pos1);
cdist = mju_dot3(tmp, con[0].frame);
mjtNum tmp[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]};
mjtNum cdist = mju_dot3(tmp, con[0].frame);
if (cdist > margin + size2[0]) {
return 0;
}
@@ -68,22 +66,19 @@ int mjc_PlaneSphere(const mjModel* m, const mjData* d,
int mjc_PlaneCapsule(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
mjtNum pos[3], axis[3], segment[3];
int n1, n2;
// get capsule axis, segment = scaled axis
axis[0] = mat2[2];
axis[1] = mat2[5];
axis[2] = mat2[8];
mju_scl3(segment, axis, size2[1]);
mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]};
mjtNum segment[3] = {size2[1]*axis[0], size2[1]*axis[1], size2[1]*axis[2]};
// get point 1, do sphere-plane test
mjtNum pos[3];
mju_add3(pos, pos2, segment);
n1 = _PlaneSphere(con, margin, pos1, mat1, size1, pos, mat2, size2);
int n1 = _PlaneSphere(con, margin, pos1, mat1, size1, pos, mat2, size2);
// get point 2, do sphere-plane test
mju_sub3(pos, pos2, segment);
n2 = _PlaneSphere(con+n1, margin, pos1, mat1, size1, pos, mat2, size2);
int n2 = _PlaneSphere(con+n1, margin, pos1, mat1, size1, pos, mat2, size2);
// align contact frames with capsule axis
if (n1) {
@@ -104,29 +99,26 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d,
mjGETINFO
mjtNum normal[3] = {mat1[2], mat1[5], mat1[8]};
mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]};
mjtNum vec[3], vec1[3];
mjtNum len, scl, dist0, prjaxis, prjvec, prjvec1;
int cnt = 0;
// project, make sure axis points towards plane
prjaxis = mju_dot3(normal, axis);
mjtNum prjaxis = mju_dot3(normal, axis);
if (prjaxis > 0) {
mju_scl3(axis, axis, -1);
prjaxis = -prjaxis;
}
// compute normal distance to cylinder center
mju_sub3(vec, pos2, pos1);
dist0 = mju_dot3(vec, normal);
mjtNum vec[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]};
mjtNum dist0 = mju_dot3(vec, normal);
// remove component of -normal along axis, compute length
mju_scl3(vec, axis, prjaxis);
mju_subFrom3(vec, normal);
len = mju_norm3(vec);
mjtNum len_sqr = mju_dot3(vec, vec);
// general configuration: normalize vector, scale by radius
if (len >= mjMINVAL) {
scl = size2[0]/len;
if (len_sqr >= mjMINVAL*mjMINVAL) {
mjtNum scl = size2[0]/mju_sqrt(len_sqr);
vec[0] *= scl;
vec[1] *= scl;
vec[2] *= scl;
@@ -140,13 +132,14 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d,
}
// project vector on normal
prjvec = mju_dot3(vec, normal);
mjtNum prjvec = mju_dot3(vec, normal);
// scale axis by half-length
mju_scl3(axis, axis, size2[1]);
prjaxis *= size2[1];
// check first point, construct contact
int cnt = 0;
if (dist0 + prjaxis + prjvec <= margin) {
con[cnt].dist = dist0 + prjaxis + prjvec;
mju_add3(con[cnt].pos, pos2, vec);
@@ -171,9 +164,10 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d,
}
// try to add triangle points on side closer to plane
prjvec1 = -prjvec*0.5;
mjtNum prjvec1 = -prjvec*0.5;
if (dist0 + prjaxis + prjvec1 <= margin) {
// compute sideways vector: vec1
mjtNum vec1[3];
mju_cross(vec1, vec, axis);
mju_normalize3(vec1);
mju_scl3(vec1, vec1, size2[0]*mju_sqrt(3.0)/2);
@@ -208,26 +202,27 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d,
int mjc_PlaneBox(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
int cnt = 0;
// get normal, difference between centers, normal distance
mjtNum norm[3] = {mat1[2], mat1[5], mat1[8]};
mjtNum dif[3], vec[3], corner[3], dist, ldist;
mju_sub3(dif, pos2, pos1);
dist = mju_dot3(dif, norm);
mjtNum dif[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]};
mjtNum dist = mju_dot3(dif, norm);
// test all corners, pick bottom 4
int cnt = 0;
for (int i=0; i < 8; i++) {
// get corner in local coordinates
mjtNum vec[3];
vec[0] = (i&1 ? size2[0] : -size2[0]);
vec[1] = (i&2 ? size2[1] : -size2[1]);
vec[2] = (i&4 ? size2[2] : -size2[2]);
// get corner in global coordinates relative to box center
mjtNum corner[3];
mju_rotVecMat(corner, vec, mat2);
// compute distance to plane, skip if too far or pointing up
ldist = mju_dot3(norm, corner);
mjtNum ldist = mju_dot3(norm, corner);
if (dist + ldist > margin || ldist > 0) {
continue;
}
@@ -255,31 +250,26 @@ int mjc_PlaneBox(const mjModel* m, const mjData* d,
// sphere : sphere (actual implementation, can be called with modified parameters)
static int _SphereSphere(mjContact* con, mjtNum margin,
mjtNum* pos1, mjtNum* mat1, mjtNum* size1,
mjtNum* pos2, mjtNum* mat2, mjtNum* size2) {
mjtNum len, cdist;
mjtNum axis1[3], axis2[3];
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
// check bounding spheres (this is called from other functions)
cdist = mju_dist3(pos1, pos2);
if (cdist > margin + size1[0] + size2[0]) {
mjtNum dif[3] = {pos1[0] - pos2[0], pos1[1] - pos2[1], pos1[2] - pos2[2]};
mjtNum cdist_sqr = mju_dot3(dif, dif);
mjtNum min_dist = margin + size1[0] + size2[0];
if (cdist_sqr > min_dist*min_dist) {
return 0;
}
// depth and normal
con[0].dist = cdist - size1[0] - size2[0];
con[0].dist = mju_sqrt(cdist_sqr) - size1[0] - size2[0];
mju_sub3(con[0].frame, pos2, pos1);
len = mju_normalize3(con[0].frame);
mjtNum len = mju_normalize3(con[0].frame);
// if centers are the same, norm = cross-product of z axes
// if z axes are parallel, norm = [1;0;0]
if (len < mjMINVAL) {
axis1[0] = mat1[2];
axis1[1] = mat1[5];
axis1[2] = mat1[8];
axis2[0] = mat2[2];
axis2[1] = mat2[5];
axis2[2] = mat2[8];
mjtNum axis1[3] = {mat1[2], mat1[5], mat1[8]};
mjtNum axis2[3] = {mat2[2], mat2[5], mat2[8]};
mju_cross(con[0].frame, axis1, axis2);
mju_normalize3(con[0].frame);
}
@@ -307,21 +297,14 @@ int mjc_SphereSphere(const mjModel* m, const mjData* d,
int mjc_SphereCapsule(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
mjtNum x, axis[3], vec[3];
// get capsule axis (scaled)
axis[0] = mat2[2] * size2[1];
axis[1] = mat2[5] * size2[1];
axis[2] = mat2[8] * size2[1];
// get capsule length and axis
mjtNum len = size2[1];
mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]};
// find projection, clip to segment
mju_sub3(vec, pos1, pos2);
x = mju_dot3(axis, vec) / mju_dot3(axis, axis);
if (x > 1) {
x = 1;
} else if (x < -1) {
x = -1;
}
mjtNum vec[3] = {pos1[0] - pos2[0], pos1[1] - pos2[1], pos1[2] - pos2[2]};
mjtNum x = mju_clip(mju_dot3(axis, vec), -len, len);
// find nearest point on segment, do sphere-sphere test
mju_scl3(vec, axis, x);
@@ -335,59 +318,43 @@ int mjc_SphereCapsule(const mjModel* m, const mjData* d,
int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
mjtNum axis1[3], axis2[3], dif[3], vec1[3], vec2[3];
mjtNum ma, mb, mc, u, v, det, x1, x2;
int n1, n2, n3, n4;
// get capsule axes (scaled) and center difference
axis1[0] = mat1[2] * size1[1];
axis1[1] = mat1[5] * size1[1];
axis1[2] = mat1[8] * size1[1];
axis2[0] = mat2[2] * size2[1];
axis2[1] = mat2[5] * size2[1];
axis2[2] = mat2[8] * size2[1];
mju_sub3(dif, pos1, pos2);
mjtNum axis1[3] = {mat1[2] * size1[1], mat1[5] * size1[1], mat1[8] * size1[1]};
mjtNum axis2[3] = {mat2[2] * size2[1], mat2[5] * size2[1], mat2[8] * size2[1]};
mjtNum dif[3] = {pos1[0] - pos2[0], pos1[1] - pos2[1], pos1[2] - pos2[2]};
// compute matrix coefficients and determinant
ma = mju_dot3(axis1, axis1);
mb = -mju_dot3(axis1, axis2);
mc = mju_dot3(axis2, axis2);
u = -mju_dot3(axis1, dif);
v = mju_dot3(axis2, dif);
det = ma*mc - mb*mb;
mjtNum ma = mju_dot3(axis1, axis1);
mjtNum mb = -mju_dot3(axis1, axis2);
mjtNum mc = mju_dot3(axis2, axis2);
mjtNum u = -mju_dot3(axis1, dif);
mjtNum v = mju_dot3(axis2, dif);
mjtNum det = ma*mc - mb*mb;
// general configuration (non-parallel axes)
if (fabs(det) >= mjMINVAL) {
// find projections, clip to segments
x1 = (mc*u - mb*v) / det;
x2 = (ma*v - mb*u) / det;
mjtNum x1 = (mc*u - mb*v) / det;
mjtNum x2 = (ma*v - mb*u) / det;
if (x1 > 1) {
x1 = 1;
x2 = (v-mb)/mc;
x2 = (v - mb) / mc;
} else if (x1 < -1) {
x1 = -1;
x2 = (v+mb)/mc;
x2 = (v + mb) / mc;
}
if (x2 > 1) {
x2 = 1;
x1 = (u-mb)/ma;
if (x1 > 1) {
x1 = 1;
} else if (x1 < -1) {
x1 = -1;
}
x1 = mju_clip((u - mb) / ma, -1, 1);
} else if (x2 < -1) {
x2 = -1;
x1 = (u+mb)/ma;
if (x1 > 1) {
x1 = 1;
} else if (x1 < -1) {
x1 = -1;
}
x1 = mju_clip((u + mb) / ma, -1, 1);
}
// find nearest points, do sphere-sphere test
mjtNum vec1[3], vec2[3];
mju_scl3(vec1, axis1, x1);
mju_addTo3(vec1, pos1);
mju_scl3(vec2, axis2, x2);
@@ -399,28 +366,21 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
// parallel axes
else {
// x1 = 1
mjtNum vec1[3];
mju_add3(vec1, pos1, axis1);
x2 = (v - mb) / mc;
if (x2 > 1) {
x2 = 1;
} else if (x2 < -1) {
x2 = -1;
}
mjtNum x2 = mju_clip((v - mb) / mc, -1, 1);
mjtNum vec2[3];
mju_scl3(vec2, axis2, x2);
mju_addTo3(vec2, pos2);
n1 = _SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2);
int n1 = _SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2);
// x1 = -1
mju_sub3(vec1, pos1, axis1);
x2 = (v + mb) / mc;
if (x2 > 1) {
x2 = 1;
} else if (x2 < -1) {
x2 = -1;
}
x2 = mju_clip((v + mb) / mc, -1, 1);
mju_scl3(vec2, axis2, x2);
mju_addTo3(vec2, pos2);
n2 = _SphereSphere(con+n1, margin, vec1, mat1, size1, vec2, mat2, size2);
int n2 = _SphereSphere(con+n1, margin, vec1, mat1, size1, vec2, mat2, size2);
// return if two contacts already found
if (n1+n2 >= 2) {
@@ -429,15 +389,10 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
// x2 = 1
mju_add3(vec2, pos2, axis2);
x1 = (u - mb) / ma;
if (x1 > 1) {
x1 = 1;
} else if (x1 < -1) {
x1 = -1;
}
mjtNum x1 = mju_clip((u - mb) / ma, -1, 1);
mju_scl3(vec1, axis1, x1);
mju_addTo3(vec1, pos1);
n3 = _SphereSphere(con+n1+n2, margin, vec1, mat1, size1, vec2, mat2, size2);
int n3 = _SphereSphere(con+n1+n2, margin, vec1, mat1, size1, vec2, mat2, size2);
// return if two contacts already found
if (n1+n2+n3 >= 2) {
@@ -446,15 +401,10 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
// x2 = -1
mju_sub3(vec2, pos2, axis2);
x1 = (u + mb) / ma;
if (x1 > 1) {
x1 = 1;
} else if (x1 < -1) {
x1 = -1;
}
x1 = mju_clip((u + mb) / ma, -1, 1);
mju_scl3(vec1, axis1, x1);
mju_addTo3(vec1, pos1);
n4 = _SphereSphere(con+n1+n2+n3, margin, vec1, mat1, size1, vec2, mat2, size2);
int n4 = _SphereSphere(con+n1+n2+n3, margin, vec1, mat1, size1, vec2, mat2, size2);
return n1+n2+n3+n4;
}