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

Change MuJoCo engine source code function-spacing convention from 3 blank lines to 2

Browse Source 
PiperOrigin-RevId: 813754244
Change-Id: I6836e41c3b021cb727e922c25c60f629b9814c93
This commit is contained in:
Yuval Tassa
2025-10-01 07:57:17 -07:00
committed by Copybara-Service
parent c439628f82
commit edbdb5195c
36 changed files with 8 additions and 651 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
-51
View File
@@ -166,7 +166,6 @@ static int discreteGeoms(mjCCDObj* obj1, mjCCDObj* obj2) {
}
// GJK algorithm
static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
int get_dist = status->dist_cutoff > 0; // need to recover geom distances if not in contact
@@ -296,7 +295,6 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
}
// compute the support point in obj1 and obj2 for Minkowski difference
static inline void support(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum dir[3], const mjtNum dir_neg[3]) {
@@ -327,7 +325,6 @@ static inline void support(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2,
}
// compute the support points in obj1 and obj2 for the kth approximation point
static inline void gjkSupport(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum x_k[3], mjtNum x_norm) {
@@ -340,7 +337,6 @@ static inline void gjkSupport(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2,
}
// compute support points in Minkowski difference, return index of new vertex in polytope
static int epaSupport(Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum d[3], mjtNum dnorm) {
@@ -361,7 +357,6 @@ static int epaSupport(Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2,
}
// compute the support point in the Minkowski difference for gjkIntersect (without normalization)
static void gjkIntersectSupport(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum dir[3]) {
@@ -370,7 +365,6 @@ static void gjkIntersectSupport(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2,
}
// compute the signed distance of a face along with the normal
static inline mjtNum signedDistance(mjtNum normal[3], const Vertex* v1, const Vertex* v2,
const Vertex* v3) {
@@ -387,7 +381,6 @@ static inline mjtNum signedDistance(mjtNum normal[3], const Vertex* v1, const Ve
}
// return 1 if objects are in contact; 0 if not; -1 if inconclusive
static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
Vertex simplex[4] = {status->simplex[0], status->simplex[1],
@@ -447,7 +440,6 @@ static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
}
// linear combination of n 3D vectors
static inline void lincomb(mjtNum res[3], const mjtNum* coef, int n, const mjtNum v1[3],
const mjtNum v2[3], const mjtNum v3[3], const mjtNum v4[3]) {
@@ -476,7 +468,6 @@ static inline void lincomb(mjtNum res[3], const mjtNum* coef, int n, const mjtNu
}
// res = origin projected onto plane defined by v1, v2, v3
static int projectOriginPlane(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3],
const mjtNum v3[3]) {
@@ -514,7 +505,6 @@ static int projectOriginPlane(mjtNum res[3], const mjtNum v1[3], const mjtNum v2
}
// res = origin projected onto line defined by v1, v2
static inline void projectOriginLine(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3]) {
// res = v2 - <v2, v2 - v1> / <v2 - v1, v2 - v1> * (v2 - v1)
@@ -527,7 +517,6 @@ static inline void projectOriginLine(mjtNum res[3], const mjtNum v1[3], const mj
}
// return 1 if both numbers are positive, -1 if both negative and 0 otherwise
static inline int sameSign2(mjtNum a, mjtNum b) {
if (a > 0 && b > 0) return 1;
@@ -536,7 +525,6 @@ static inline int sameSign2(mjtNum a, mjtNum b) {
}
// subdistance algorithm for GJK that computes the barycentric coordinates of the point in a
// simplex closest to the origin
// implementation adapted from Montanari et al, ToG 2017
@@ -564,7 +552,6 @@ static inline void subdistance(mjtNum lambda[4], int n, const Vertex simplex[4])
}
static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3],
const mjtNum s3[3], const mjtNum s4[3]) {
// the matrix M is given by
@@ -656,7 +643,6 @@ static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3],
}
static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const mjtNum s3[3]) {
// project origin onto affine hull of the simplex
mjtNum p_o[3];
@@ -790,7 +776,6 @@ static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const
}
static void S1D(mjtNum lambda[2], const mjtNum s1[3], const mjtNum s2[3]) {
// find projection of origin onto the 1-simplex:
mjtNum p_o[3];
@@ -840,7 +825,6 @@ static inline void replaceSimplex3(Polytope* pt, mjCCDStatus* status, int v1, in
}
// return 1 if the origin and p3 are on the same side of the plane defined by p0, p1, p2
static int sameSide(const mjtNum p0[3], const mjtNum p1[3],
const mjtNum p2[3], const mjtNum p3[3]) {
@@ -860,7 +844,6 @@ static int sameSide(const mjtNum p0[3], const mjtNum p1[3],
}
// return 1 if the origin is contained in the tetrahedron, 0 otherwise
static int testTetra(const mjtNum p0[3], const mjtNum p1[3],
const mjtNum p2[3], const mjtNum p3[3]) {
@@ -871,7 +854,6 @@ static int testTetra(const mjtNum p0[3], const mjtNum p1[3],
}
// matrix for 120 degrees rotation around given axis
static void rotmat(mjtNum R[9], const mjtNum axis[3]) {
mjtNum n = norm3(axis);
@@ -890,7 +872,6 @@ static void rotmat(mjtNum R[9], const mjtNum axis[3]) {
}
// return nonzero if the ray v1v2 intersects the triangle v3v4v5
static inline int rayTriangle(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3],
const mjtNum v4[3], const mjtNum v5[3]) {
@@ -910,7 +891,6 @@ static inline int rayTriangle(const mjtNum v1[3], const mjtNum v2[3], const mjtN
}
// create a polytope from a 1-simplex (returns 0 on success)
static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum *v1 = status->simplex[0].vert, *v2 = status->simplex[1].vert;
@@ -994,7 +974,6 @@ static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
}
// compute the affine coordinates of p on the triangle v1v2v3
static void triAffineCoord(mjtNum lambda[3], const mjtNum v1[3], const mjtNum v2[3],
const mjtNum v3[3], const mjtNum p[3]) {
@@ -1040,7 +1019,6 @@ static void triAffineCoord(mjtNum lambda[3], const mjtNum v1[3], const mjtNum v2
}
// return true if point p and triangle v1v2v3 intersect
static int triPointIntersect(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3],
const mjtNum p[3]) {
@@ -1058,7 +1036,6 @@ static int triPointIntersect(const mjtNum v1[3], const mjtNum v2[3], const mjtNu
}
// create a polytope from a 2-simplex (returns 0 on success)
static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
// get vertices of simplex from GJK
@@ -1138,7 +1115,6 @@ static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
}
// create a polytope from a 3-simplex (returns 0 on success)
static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
int v1 = insertVertex(pt, status->simplex + 0);
@@ -1196,14 +1172,12 @@ static void deleteFace(Polytope* pt, Face* face) {
}
// return max number of faces that can be stored in polytope
static inline int maxFaces(Polytope* pt) {
return pt->maxfaces - pt->nfaces;
}
// attach a face to the polytope with the given vertex indices; return squared distance to origin
static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3,
int adj1, int adj2, int adj3) {
@@ -1229,7 +1203,6 @@ static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3,
}
// add an edge to the horizon
static inline void addEdge(Polytope* pt, int index, int edge) {
pt->horizon.edges[pt->horizon.nedges] = edge;
@@ -1237,7 +1210,6 @@ static inline void addEdge(Polytope* pt, int index, int edge) {
}
// get edge index where vertex lies
static inline int getEdge(Face* face, int vertex) {
if (face->verts[0] == vertex) return 0;
@@ -1246,7 +1218,6 @@ static inline int getEdge(Face* face, int vertex) {
}
// recursive call to build horizon; return 1 if face is visible from w otherwise 0
static int horizonRec(Polytope* pt, Face* face, int e) {
// v is visible from w so it is deleted and adjacent faces are checked
@@ -1270,7 +1241,6 @@ static int horizonRec(Polytope* pt, Face* face, int e) {
}
// create horizon given the face as starting point
static void horizon(Polytope* pt, Face* face) {
deleteFace(pt, face);
@@ -1298,7 +1268,6 @@ static void horizon(Polytope* pt, Face* face) {
}
// recover witness points from EPA polytope
static void epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNum x2[3]) {
// compute affine coordinates for witness points on plane defined by face
@@ -1326,7 +1295,6 @@ static void epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNu
}
// return a face of the expanded polytope that best approximates the pentration depth
// witness points are in status->{x1, x2}
static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) {
@@ -1493,7 +1461,6 @@ static inline mjtNum area4(const mjtNum a[3], const mjtNum b[3],
}
// return pointer to next vertex in a polygon
static inline mjtNum* next(mjtNum* polygon, int nvert, mjtNum* curr) {
if (curr == polygon + 3*(nvert - 1)) {
@@ -1503,7 +1470,6 @@ static inline mjtNum* next(mjtNum* polygon, int nvert, mjtNum* curr) {
}
// prune a polygon to a maximum area convex quadrilateral
static inline void polygonQuad(mjtNum* res[4], mjtNum* polygon, int nvert) {
mjtNum* a = polygon, *b = polygon + 3, *c = polygon + 6, *d = polygon + 9;
@@ -1551,7 +1517,6 @@ static inline void polygonQuad(mjtNum* res[4], mjtNum* polygon, int nvert) {
}
// find the normal of a plane perpendicular to the face (given by its normal n) and intersecting the
// face edge (v1, v2)
static mjtNum planeNormal(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3],
@@ -1565,7 +1530,6 @@ static mjtNum planeNormal(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3],
}
// find what side of a plane a point p lies
static int halfspace(const mjtNum a[3], const mjtNum n[3], const mjtNum p[3]) {
mjtNum diff[3] = {p[0] - a[0], p[1] - a[1], p[2] - a[2]};
@@ -1573,7 +1537,6 @@ static int halfspace(const mjtNum a[3], const mjtNum n[3], const mjtNum p[3]) {
}
// compute the intersection of a plane with a line segment (a, b)
static mjtNum planeIntersect(mjtNum res[3], const mjtNum pn[3], mjtNum pd,
const mjtNum a[3], const mjtNum b[3]) {
@@ -1591,7 +1554,6 @@ static mjtNum planeIntersect(mjtNum res[3], const mjtNum pn[3], mjtNum pd,
}
// clip a polygon against another polygon
static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1,
const mjtNum* face2, int nface2, const mjtNum n[3],
@@ -1707,7 +1669,6 @@ static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1,
}
// compute global coordinates of a local point (l1, l2, l3)
static inline void globalcoord(mjtNum res[3], const mjtNum mat[9], const mjtNum pos[3],
mjtNum l1, mjtNum l2, mjtNum l3) {
@@ -1723,7 +1684,6 @@ static inline void globalcoord(mjtNum res[3], const mjtNum mat[9], const mjtNum
}
// find up to n <= 2 common integers of two arrays, return n
static int intersect(int res[2], const int* arr1, const int* arr2, int n, int m) {
int count = 0;
@@ -1739,7 +1699,6 @@ static int intersect(int res[2], const int* arr1, const int* arr2, int n, int m)
}
// compute possible polygon normals of a mesh given up to 3 vertices
static int meshNormals(mjtNum* res, int resind[3], int dim, mjCCDObj* obj,
int v1, int v2, int v3) {
@@ -1810,7 +1769,6 @@ static int meshNormals(mjtNum* res, int resind[3], int dim, mjCCDObj* obj,
}
// compute normal directional vectors along possible edges given by up to two vertices
static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj,
const mjtNum v1[3], const mjtNum v2[3], int v1i, int v2i) {
@@ -1865,7 +1823,6 @@ static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj
}
// try recovering box normal from collision normal
static int boxNormals2(mjtNum res[9], int resind[3], const mjtNum mat[9], const mjtNum n[3]) {
// list of box face normals
@@ -1892,7 +1849,6 @@ static int boxNormals2(mjtNum res[9], int resind[3], const mjtNum mat[9], const
}
// compute possible face normals of a box given up to 3 vertices
static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj,
int v1, int v2, int v3, const mjtNum dir[3]) {
@@ -1947,7 +1903,6 @@ static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj,
}
// compute possible edge normals for box for edge collisions
static int boxEdgeNormals(mjtNum res[9], mjtNum endverts[9], int dim, mjCCDObj* obj,
const mjtNum v1[3], const mjtNum v2[3], int v1i, int v2i) {
@@ -2038,7 +1993,6 @@ static int boxFace(mjtNum res[12], mjCCDObj* obj, int idx) {
}
// recover mesh polygon from its index, return number of edges
static int meshFace(mjtNum* res, mjCCDObj* obj, int idx) {
const mjModel* m = obj->model;
@@ -2063,7 +2017,6 @@ static int meshFace(mjtNum* res, mjCCDObj* obj, int idx) {
}
// find two normals that are facing each other within a tolerance, return 1 if found
static inline int alignedFaces(int res[2], const mjtNum* v, int nv,
const mjtNum* w, int nw) {
@@ -2080,7 +2033,6 @@ static inline int alignedFaces(int res[2], const mjtNum* v, int nv,
}
// find two normals that are perpendicular to each other within a tolerance, return 1 if found
static inline int alignedFaceEdge(int res[2], const mjtNum* edge, int nedge,
const mjtNum* face, int nface) {
@@ -2115,7 +2067,6 @@ static inline int simplexDim(int* v1i, int* v2i, int* v3i, mjtNum** v1, mjtNum**
}
// recover multiple contacts from EPA polytope
static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status,
mjCCDObj* obj1, mjCCDObj* obj2) {
@@ -2242,7 +2193,6 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status,
// inflate a contact by margin
static inline void inflate(mjCCDStatus* status, mjtNum margin1, mjtNum margin2) {
mjtNum n[3];
@@ -2262,7 +2212,6 @@ static inline void inflate(mjCCDStatus* status, mjtNum margin1, mjtNum margin2)
}
// general convex collision detection
mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
// pre-allocate static memory for low iterations