From 1d6d6f33bf1d2a7ae131e34b0895d3610a14e6d8 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 26 Jun 2023 15:35:40 -0700 Subject: [PATCH] Cosmetic clean-up and a few micro-optimisations for primitive colliders. PiperOrigin-RevId: 543559919 Change-Id: I345ccbdca4303e82d1097a5a4bf9381c9f845617 --- src/engine/engine_collision_primitive.c | 186 +++++++++--------------- 1 file changed, 68 insertions(+), 118 deletions(-) diff --git a/src/engine/engine_collision_primitive.c b/src/engine/engine_collision_primitive.c index 0052f987..ad03a9ba 100644 --- a/src/engine/engine_collision_primitive.c +++ b/src/engine/engine_collision_primitive.c @@ -20,6 +20,7 @@ #include #include #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; }