Use mj_rayMesh for computing an inexact SDF for mesh-SDF collisions.
PiperOrigin-RevId: 794576123 Change-Id: I3c688056a5ed617a0787ccf1f4aa683f9409040f
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
cc52d0242b
commit
dcdf0a9c5c
@@ -0,0 +1,296 @@
|
||||
v 0.740000 0.740000 -1.000000
|
||||
v 0.740000 1.000000 -0.740000
|
||||
v 1.000000 0.740000 -0.740000
|
||||
v 0.740000 0.807269 -0.991147
|
||||
v 0.740000 0.870021 -0.965154
|
||||
v 0.740000 0.923848 -0.923848
|
||||
v 0.807269 0.740000 -0.991147
|
||||
v 0.809387 0.809958 -0.980606
|
||||
v 0.808254 0.870420 -0.954318
|
||||
v 0.806858 0.917583 -0.917748
|
||||
v 0.870021 0.740000 -0.965154
|
||||
v 0.870490 0.808646 -0.954150
|
||||
v 0.863847 0.863908 -0.932116
|
||||
v 0.858399 0.903670 -0.903688
|
||||
v 0.890111 0.890111 -0.890111
|
||||
v 0.807269 0.991147 -0.740000
|
||||
v 0.870021 0.965154 -0.740000
|
||||
v 0.923848 0.923848 -0.740000
|
||||
v 0.740000 0.991147 -0.807269
|
||||
v 0.809958 0.980606 -0.809387
|
||||
v 0.870420 0.954318 -0.808254
|
||||
v 0.917583 0.917748 -0.806858
|
||||
v 0.740000 0.965154 -0.870021
|
||||
v 0.808646 0.954150 -0.870490
|
||||
v 0.863908 0.932116 -0.863847
|
||||
v 0.903670 0.903688 -0.858399
|
||||
v 0.991147 0.740000 -0.807269
|
||||
v 0.965154 0.740000 -0.870021
|
||||
v 0.923848 0.740000 -0.923848
|
||||
v 0.991147 0.807269 -0.740000
|
||||
v 0.980606 0.809387 -0.809958
|
||||
v 0.954318 0.808254 -0.870420
|
||||
v 0.917748 0.806858 -0.917583
|
||||
v 0.965154 0.870021 -0.740000
|
||||
v 0.954150 0.870490 -0.808646
|
||||
v 0.932116 0.863847 -0.863908
|
||||
v 0.903688 0.858399 -0.903670
|
||||
v 0.740000 -1.000000 -0.740000
|
||||
v 0.740000 -0.740000 -1.000000
|
||||
v 1.000000 -0.740000 -0.740000
|
||||
v 0.740000 -0.991147 -0.807269
|
||||
v 0.740000 -0.965154 -0.870021
|
||||
v 0.740000 -0.923848 -0.923848
|
||||
v 0.807269 -0.991147 -0.740000
|
||||
v 0.809387 -0.980606 -0.809958
|
||||
v 0.808254 -0.954318 -0.870420
|
||||
v 0.806858 -0.917748 -0.917583
|
||||
v 0.870021 -0.965154 -0.740000
|
||||
v 0.870490 -0.954150 -0.808646
|
||||
v 0.863847 -0.932116 -0.863908
|
||||
v 0.858399 -0.903688 -0.903670
|
||||
v 0.890111 -0.890111 -0.890111
|
||||
v 0.807269 -0.740000 -0.991147
|
||||
v 0.870021 -0.740000 -0.965154
|
||||
v 0.923848 -0.740000 -0.923848
|
||||
v 0.740000 -0.807269 -0.991147
|
||||
v 0.809958 -0.809387 -0.980606
|
||||
v 0.870420 -0.808254 -0.954318
|
||||
v 0.917583 -0.806858 -0.917748
|
||||
v 0.740000 -0.870021 -0.965154
|
||||
v 0.808646 -0.870490 -0.954150
|
||||
v 0.863908 -0.863847 -0.932116
|
||||
v 0.903670 -0.858399 -0.903688
|
||||
v 0.991147 -0.807269 -0.740000
|
||||
v 0.965154 -0.870021 -0.740000
|
||||
v 0.923848 -0.923848 -0.740000
|
||||
v 0.991147 -0.740000 -0.807269
|
||||
v 0.980606 -0.809958 -0.809387
|
||||
v 0.954318 -0.870420 -0.808254
|
||||
v 0.917748 -0.917583 -0.806858
|
||||
v 0.965154 -0.740000 -0.870021
|
||||
v 0.954150 -0.808646 -0.870490
|
||||
v 0.932116 -0.863908 -0.863847
|
||||
v 0.903688 -0.903670 -0.858399
|
||||
v 1.000000 0.740000 0.740000
|
||||
v 0.740000 1.000000 0.740000
|
||||
v 0.740000 0.740000 1.000000
|
||||
v 0.991147 0.807269 0.740000
|
||||
v 0.965154 0.870021 0.740000
|
||||
v 0.923848 0.923848 0.740000
|
||||
v 0.991147 0.740000 0.807269
|
||||
v 0.980606 0.809958 0.809387
|
||||
v 0.954318 0.870420 0.808254
|
||||
v 0.917748 0.917583 0.806858
|
||||
v 0.965154 0.740000 0.870021
|
||||
v 0.954150 0.808646 0.870490
|
||||
v 0.932116 0.863908 0.863847
|
||||
v 0.903688 0.903670 0.858399
|
||||
v 0.890111 0.890111 0.890111
|
||||
v 0.740000 0.991147 0.807269
|
||||
v 0.740000 0.965154 0.870021
|
||||
v 0.740000 0.923848 0.923848
|
||||
v 0.807269 0.991147 0.740000
|
||||
v 0.809387 0.980606 0.809958
|
||||
v 0.808254 0.954318 0.870420
|
||||
v 0.806858 0.917748 0.917583
|
||||
v 0.870021 0.965154 0.740000
|
||||
v 0.870490 0.954150 0.808646
|
||||
v 0.863847 0.932116 0.863908
|
||||
v 0.858399 0.903688 0.903670
|
||||
v 0.807269 0.740000 0.991147
|
||||
v 0.870021 0.740000 0.965154
|
||||
v 0.923848 0.740000 0.923848
|
||||
v 0.740000 0.807269 0.991147
|
||||
v 0.809958 0.809387 0.980606
|
||||
v 0.870420 0.808254 0.954318
|
||||
v 0.917583 0.806858 0.917748
|
||||
v 0.740000 0.870021 0.965154
|
||||
v 0.808646 0.870490 0.954150
|
||||
v 0.863908 0.863847 0.932116
|
||||
v 0.903670 0.858399 0.903688
|
||||
v 1.000000 -0.740000 0.740000
|
||||
v 0.740000 -0.740000 1.000000
|
||||
v 0.740000 -1.000000 0.740000
|
||||
v 0.991147 -0.740000 0.807269
|
||||
v 0.965154 -0.740000 0.870021
|
||||
v 0.923848 -0.740000 0.923848
|
||||
v 0.991147 -0.807269 0.740000
|
||||
v 0.980606 -0.809387 0.809958
|
||||
v 0.954318 -0.808254 0.870420
|
||||
v 0.917748 -0.806858 0.917583
|
||||
v 0.965154 -0.870021 0.740000
|
||||
v 0.954150 -0.870490 0.808646
|
||||
v 0.932116 -0.863847 0.863908
|
||||
v 0.903688 -0.858399 0.903670
|
||||
v 0.890111 -0.890111 0.890111
|
||||
v 0.740000 -0.807269 0.991147
|
||||
v 0.740000 -0.870021 0.965154
|
||||
v 0.740000 -0.923848 0.923848
|
||||
v 0.807269 -0.740000 0.991147
|
||||
v 0.809387 -0.809958 0.980606
|
||||
v 0.808254 -0.870420 0.954318
|
||||
v 0.806858 -0.917583 0.917748
|
||||
v 0.870021 -0.740000 0.965154
|
||||
v 0.870490 -0.808646 0.954150
|
||||
v 0.863847 -0.863908 0.932116
|
||||
v 0.858399 -0.903670 0.903688
|
||||
v 0.807269 -0.991147 0.740000
|
||||
v 0.870021 -0.965154 0.740000
|
||||
v 0.923848 -0.923848 0.740000
|
||||
v 0.740000 -0.991147 0.807269
|
||||
v 0.809958 -0.980606 0.809387
|
||||
v 0.870420 -0.954318 0.808254
|
||||
v 0.917583 -0.917748 0.806858
|
||||
v 0.740000 -0.965154 0.870021
|
||||
v 0.808646 -0.954150 0.870490
|
||||
v 0.863908 -0.932116 0.863847
|
||||
v 0.903670 -0.903688 0.858399
|
||||
v -0.740000 0.740000 -1.000000
|
||||
v -1.000000 0.740000 -0.740000
|
||||
v -0.740000 1.000000 -0.740000
|
||||
v -0.807269 0.740000 -0.991147
|
||||
v -0.870021 0.740000 -0.965154
|
||||
v -0.923848 0.740000 -0.923848
|
||||
v -0.740000 0.807269 -0.991147
|
||||
v -0.809958 0.809387 -0.980606
|
||||
v -0.870420 0.808254 -0.954318
|
||||
v -0.917583 0.806858 -0.917748
|
||||
v -0.740000 0.870021 -0.965154
|
||||
v -0.808646 0.870490 -0.954150
|
||||
v -0.863908 0.863847 -0.932116
|
||||
v -0.903670 0.858399 -0.903688
|
||||
v -0.890111 0.890111 -0.890111
|
||||
v -0.991147 0.807269 -0.740000
|
||||
v -0.965154 0.870021 -0.740000
|
||||
v -0.923848 0.923848 -0.740000
|
||||
v -0.991147 0.740000 -0.807269
|
||||
v -0.980606 0.809958 -0.809387
|
||||
v -0.954318 0.870420 -0.808254
|
||||
v -0.917748 0.917583 -0.806858
|
||||
v -0.965154 0.740000 -0.870021
|
||||
v -0.954150 0.808646 -0.870490
|
||||
v -0.932116 0.863908 -0.863847
|
||||
v -0.903688 0.903670 -0.858399
|
||||
v -0.740000 0.991147 -0.807269
|
||||
v -0.740000 0.965154 -0.870021
|
||||
v -0.740000 0.923848 -0.923848
|
||||
v -0.807269 0.991147 -0.740000
|
||||
v -0.809387 0.980606 -0.809958
|
||||
v -0.808254 0.954318 -0.870420
|
||||
v -0.806858 0.917748 -0.917583
|
||||
v -0.870021 0.965154 -0.740000
|
||||
v -0.870490 0.954150 -0.808646
|
||||
v -0.863847 0.932116 -0.863908
|
||||
v -0.858399 0.903688 -0.903670
|
||||
v -1.000000 -0.740000 -0.740000
|
||||
v -0.740000 -0.740000 -1.000000
|
||||
v -0.740000 -1.000000 -0.740000
|
||||
v -0.991147 -0.740000 -0.807269
|
||||
v -0.965154 -0.740000 -0.870021
|
||||
v -0.923848 -0.740000 -0.923848
|
||||
v -0.991147 -0.807269 -0.740000
|
||||
v -0.980606 -0.809387 -0.809958
|
||||
v -0.954318 -0.808254 -0.870420
|
||||
v -0.917748 -0.806858 -0.917583
|
||||
v -0.965154 -0.870021 -0.740000
|
||||
v -0.954150 -0.870490 -0.808646
|
||||
v -0.932116 -0.863847 -0.863908
|
||||
v -0.903688 -0.858399 -0.903670
|
||||
v -0.890111 -0.890111 -0.890111
|
||||
v -0.740000 -0.807269 -0.991147
|
||||
v -0.740000 -0.870021 -0.965154
|
||||
v -0.740000 -0.923848 -0.923848
|
||||
v -0.807269 -0.740000 -0.991147
|
||||
v -0.809387 -0.809958 -0.980606
|
||||
v -0.808254 -0.870420 -0.954318
|
||||
v -0.806858 -0.917583 -0.917748
|
||||
v -0.870021 -0.740000 -0.965154
|
||||
v -0.870490 -0.808646 -0.954150
|
||||
v -0.863847 -0.863908 -0.932116
|
||||
v -0.858399 -0.903670 -0.903688
|
||||
v -0.807269 -0.991147 -0.740000
|
||||
v -0.870021 -0.965154 -0.740000
|
||||
v -0.923848 -0.923848 -0.740000
|
||||
v -0.740000 -0.991147 -0.807269
|
||||
v -0.809958 -0.980606 -0.809387
|
||||
v -0.870420 -0.954318 -0.808254
|
||||
v -0.917583 -0.917748 -0.806858
|
||||
v -0.740000 -0.965154 -0.870021
|
||||
v -0.808646 -0.954150 -0.870490
|
||||
v -0.863908 -0.932116 -0.863847
|
||||
v -0.903670 -0.903688 -0.858399
|
||||
v -1.000000 0.740000 0.740000
|
||||
v -0.740000 0.740000 1.000000
|
||||
v -0.740000 1.000000 0.740000
|
||||
v -0.991147 0.740000 0.807269
|
||||
v -0.965154 0.740000 0.870021
|
||||
v -0.923848 0.740000 0.923848
|
||||
v -0.991147 0.807269 0.740000
|
||||
v -0.980606 0.809387 0.809958
|
||||
v -0.954318 0.808254 0.870420
|
||||
v -0.917748 0.806858 0.917583
|
||||
v -0.965154 0.870021 0.740000
|
||||
v -0.954150 0.870490 0.808646
|
||||
v -0.932116 0.863847 0.863908
|
||||
v -0.903688 0.858399 0.903670
|
||||
v -0.890111 0.890111 0.890111
|
||||
v -0.740000 0.807269 0.991147
|
||||
v -0.740000 0.870021 0.965154
|
||||
v -0.740000 0.923848 0.923848
|
||||
v -0.807269 0.740000 0.991147
|
||||
v -0.809387 0.809958 0.980606
|
||||
v -0.808254 0.870420 0.954318
|
||||
v -0.806858 0.917583 0.917748
|
||||
v -0.870021 0.740000 0.965154
|
||||
v -0.870490 0.808646 0.954150
|
||||
v -0.863847 0.863908 0.932116
|
||||
v -0.858399 0.903670 0.903688
|
||||
v -0.807269 0.991147 0.740000
|
||||
v -0.870021 0.965154 0.740000
|
||||
v -0.923848 0.923848 0.740000
|
||||
v -0.740000 0.991147 0.807269
|
||||
v -0.809958 0.980606 0.809387
|
||||
v -0.870420 0.954318 0.808254
|
||||
v -0.917583 0.917748 0.806858
|
||||
v -0.740000 0.965154 0.870021
|
||||
v -0.808646 0.954150 0.870490
|
||||
v -0.863908 0.932116 0.863847
|
||||
v -0.903670 0.903688 0.858399
|
||||
v -0.740000 -1.000000 0.740000
|
||||
v -0.740000 -0.740000 1.000000
|
||||
v -1.000000 -0.740000 0.740000
|
||||
v -0.740000 -0.991147 0.807269
|
||||
v -0.740000 -0.965154 0.870021
|
||||
v -0.740000 -0.923848 0.923848
|
||||
v -0.807269 -0.991147 0.740000
|
||||
v -0.809387 -0.980606 0.809958
|
||||
v -0.808254 -0.954318 0.870420
|
||||
v -0.806858 -0.917748 0.917583
|
||||
v -0.870021 -0.965154 0.740000
|
||||
v -0.870490 -0.954150 0.808646
|
||||
v -0.863847 -0.932116 0.863908
|
||||
v -0.858399 -0.903688 0.903670
|
||||
v -0.890111 -0.890111 0.890111
|
||||
v -0.807269 -0.740000 0.991147
|
||||
v -0.870021 -0.740000 0.965154
|
||||
v -0.923848 -0.740000 0.923848
|
||||
v -0.740000 -0.807269 0.991147
|
||||
v -0.809958 -0.809387 0.980606
|
||||
v -0.870420 -0.808254 0.954318
|
||||
v -0.917583 -0.806858 0.917748
|
||||
v -0.740000 -0.870021 0.965154
|
||||
v -0.808646 -0.870490 0.954150
|
||||
v -0.863908 -0.863847 0.932116
|
||||
v -0.903670 -0.858399 0.903688
|
||||
v -0.991147 -0.807269 0.740000
|
||||
v -0.965154 -0.870021 0.740000
|
||||
v -0.923848 -0.923848 0.740000
|
||||
v -0.991147 -0.740000 0.807269
|
||||
v -0.980606 -0.809958 0.809387
|
||||
v -0.954318 -0.870420 0.808254
|
||||
v -0.917748 -0.917583 0.806858
|
||||
v -0.965154 -0.740000 0.870021
|
||||
v -0.954150 -0.808646 0.870490
|
||||
v -0.932116 -0.863908 0.863847
|
||||
v -0.903688 -0.903670 0.858399
|
||||
@@ -0,0 +1,34 @@
|
||||
<mujoco>
|
||||
<extension>
|
||||
<plugin plugin="mujoco.sdf.torus">
|
||||
<instance name="torus">
|
||||
<config key="radius1" value="0.35"/>
|
||||
<config key="radius2" value="0.15"/>
|
||||
</instance>
|
||||
</plugin>
|
||||
</extension>
|
||||
|
||||
<option gravity="0 0 -9.81" sdf_iterations="3" sdf_initpoints="10"/>
|
||||
<asset>
|
||||
<mesh name="torus">
|
||||
<plugin instance="torus"/>
|
||||
</mesh>
|
||||
<mesh name="die" file="asset/die.obj" scale="1 1 1"/>
|
||||
</asset>
|
||||
|
||||
<include file="scene.xml"/>
|
||||
|
||||
<worldbody>
|
||||
<body pos="0 .05 2.5" euler="90 0 0">
|
||||
<freejoint/>
|
||||
<geom type="sdf" mesh="torus" rgba=".8 .17 .15 1" group="1">
|
||||
<plugin instance="torus"/>
|
||||
</geom>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="mesh" mesh="die" euler="90 0 0" rgba="0 0 1 .2"/>
|
||||
</body>
|
||||
<light pos="1 0 7" dir="0 0 -1" castshadow="false"/>
|
||||
<light pos="-1 0 7" dir="0 0 -1" castshadow="false"/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -27,7 +27,7 @@
|
||||
</geom>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="mesh" mesh="mug" euler="90 0 0" rgba="0 0 1 .2"/>
|
||||
<geom type="sdf" mesh="mug" euler="90 0 0" rgba="0 0 1 .2"/>
|
||||
</body>
|
||||
<light pos="1 0 7" dir="0 0 -1" castshadow="false"/>
|
||||
<light pos="-1 0 7" dir="0 0 -1" castshadow="false"/>
|
||||
|
||||
@@ -14,6 +14,7 @@
|
||||
|
||||
#include "engine/engine_collision_sdf.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
@@ -23,17 +24,13 @@
|
||||
#include "engine/engine_collision_primitive.h"
|
||||
#include "engine/engine_io.h"
|
||||
#include "engine/engine_plugin.h"
|
||||
#include "engine/engine_sort.h"
|
||||
#include "engine/engine_ray.h"
|
||||
#include "engine/engine_util_blas.h"
|
||||
#include "engine/engine_util_errmem.h"
|
||||
#include "engine/engine_util_misc.h"
|
||||
#include "engine/engine_util_spatial.h"
|
||||
|
||||
|
||||
#define MAXSDFFACE 1300
|
||||
#define MAXMESHPNT 500
|
||||
|
||||
|
||||
//---------------------------- interpolated sdf -------------------------------------------
|
||||
|
||||
mjtNum boxProjection(mjtNum point[3], const mjtNum box[6]) {
|
||||
@@ -274,7 +271,18 @@ static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p
|
||||
return oct_distance(m, x, i);
|
||||
}
|
||||
case mjGEOM_MESH:
|
||||
return oct_distance(m, x, i);
|
||||
if (m->mesh_octnum[i]) {
|
||||
return oct_distance(m, x, i);
|
||||
} else {
|
||||
mjtNum dir[3] = {-x[0], -x[1], -x[2]};
|
||||
mjtNum r = mju_normalize3(dir);
|
||||
mjtNum dist = mj_rayMesh(m, d, i, x, dir);
|
||||
if (dist > r) {
|
||||
mju_scl3(dir, dir, -1);
|
||||
return -mj_rayMesh(m, d, i, x, dir);
|
||||
}
|
||||
return dist;
|
||||
}
|
||||
default:
|
||||
mjERROR("sdf collisions not available for geom type %d", type);
|
||||
return 0;
|
||||
@@ -380,7 +388,16 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d,
|
||||
}
|
||||
break;
|
||||
case mjGEOM_MESH:
|
||||
oct_gradient(m, gradient, x, i);
|
||||
if (m->mesh_octnum[i]) {
|
||||
oct_gradient(m, gradient, x, i);
|
||||
} else {
|
||||
mjtNum dir[3] = {-x[0], -x[1], -x[2]};
|
||||
mjtNum r = mju_normalize3(dir);
|
||||
mjtNum dist = mj_rayMesh(m, d, i, x, dir);
|
||||
gradient[0] = dist > r ? 1 : -1;
|
||||
gradient[1] = dist > r ? 1 : -1;
|
||||
gradient[2] = dist > r ? 1 : -1;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
mjERROR("sdf collisions not available for geom type %d", type);
|
||||
@@ -493,22 +510,6 @@ static void mapPose(const mjtNum xpos1[3], const mjtNum xquat1[4],
|
||||
|
||||
//---------------------------- narrow phase -----------------------------------------------
|
||||
|
||||
// comparison function for contact sorting
|
||||
static inline int distcmp(int* i, int* j, void* context) {
|
||||
mjtNum d1 = ((mjtNum*)context)[*i];
|
||||
mjtNum d2 = ((mjtNum*)context)[*j];
|
||||
if (d1 < d2) {
|
||||
return -1;
|
||||
} else if (d1 == d2) {
|
||||
return 0;
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
// define distSort function for contact sorting
|
||||
mjSORT(distSort, int, distcmp)
|
||||
|
||||
// check if the collision point already exists
|
||||
static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) {
|
||||
for (int i = 0; i < cnt; i++) {
|
||||
@@ -548,36 +549,6 @@ static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3],
|
||||
return cnt+1;
|
||||
}
|
||||
|
||||
// finds minimum of Frank-Wolfe objective
|
||||
static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners,
|
||||
const mjModel* m, const mjSDF* sdf, mjData* d) {
|
||||
for (int step=0; step < m->opt.sdf_iterations; step++) {
|
||||
mjtNum best = 1e10, fun, s[3], grad[3];
|
||||
|
||||
// evaluate gradient
|
||||
mjc_gradient(m, d, sdf, grad, x);
|
||||
|
||||
// evaluate all corners
|
||||
for (int i=0; i < ncorners; i++) {
|
||||
// compute sdf
|
||||
fun = mju_dot3(corners + 3*i, grad);
|
||||
|
||||
// save argmin
|
||||
if (fun < best) {
|
||||
best = fun;
|
||||
mju_copy3(s, corners + 3*i);
|
||||
}
|
||||
}
|
||||
|
||||
// update collision point
|
||||
mju_subFrom3(s, x);
|
||||
mju_addToScl3(x, s, 2. / (step+2.));
|
||||
}
|
||||
|
||||
// compute distance
|
||||
return mjc_distance(m, d, sdf, x);
|
||||
}
|
||||
|
||||
// finds minimum using gradient descent
|
||||
static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s,
|
||||
mjData* d, int niter) {
|
||||
@@ -626,129 +597,7 @@ static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s,
|
||||
return dist;
|
||||
}
|
||||
|
||||
//---------------------------- bounding box vs sdf -------------------------------------------------
|
||||
|
||||
// stricter triangle collision
|
||||
static int triangleIntersect(const mjtNum triangle[9], const mjModel* m,
|
||||
const mjSDF* sdf, mjData* d) {
|
||||
mjtNum edges[6];
|
||||
mjtNum normal[3], center[3];
|
||||
mjtNum v[9], cross[9], p[3];
|
||||
mjtNum kDistanceScl = 10.;
|
||||
|
||||
// triangle normal
|
||||
mju_sub3(edges+0, triangle+3, triangle);
|
||||
mju_sub3(edges+3, triangle+6, triangle);
|
||||
mju_cross(normal, edges, edges+3);
|
||||
mju_normalize3(normal);
|
||||
|
||||
// fourth point
|
||||
mju_scl3(p, triangle, 1./3.);
|
||||
mju_addToScl3(p, triangle+3, 1./3.);
|
||||
mju_addToScl3(p, triangle+6, 1./3.);
|
||||
mjtNum h = -mjc_distance(m, d, sdf, p)/kDistanceScl;
|
||||
mju_addToScl3(p, normal, -h);
|
||||
|
||||
// circumsphere center
|
||||
mju_sub3(v+0, triangle+0, p);
|
||||
mju_sub3(v+3, triangle+3, p);
|
||||
mju_sub3(v+6, triangle+6, p);
|
||||
mju_cross(cross+0, v+3, v+6);
|
||||
mju_cross(cross+3, v+6, v+0);
|
||||
mju_cross(cross+6, v+0, v+3);
|
||||
mju_scl3(center, cross, mju_dot3(v, v));
|
||||
mju_addToScl3(center, cross+3, mju_dot3(v+3, v+3));
|
||||
mju_addToScl3(center, cross+6, mju_dot3(v+6, v+6));
|
||||
mju_scl3(center, center, 1./(2.*mju_dot3(v, cross)));
|
||||
|
||||
// circumsphere radius
|
||||
mjtNum r = mju_sqrt(mju_dot3(center, center));
|
||||
|
||||
// coordinate change
|
||||
mju_addTo3(center, p);
|
||||
|
||||
return mjc_distance(m, d, sdf, center) < r;
|
||||
}
|
||||
|
||||
// intersect with circumsphere of bounding box
|
||||
static int boxIntersect(const mjtNum bvh[6], const mjtNum offset[3],
|
||||
const mjtNum rotation[9], const mjModel* m,
|
||||
const mjSDF* s, mjData* d) {
|
||||
mjtNum candidate[3];
|
||||
mjtNum r = mju_norm3(bvh+3);
|
||||
|
||||
mju_mulMatVec3(candidate, rotation, bvh);
|
||||
mju_addTo3(candidate, offset);
|
||||
|
||||
// check if inside the bounding box
|
||||
return mjc_distance(m, d, s, candidate) < r;
|
||||
}
|
||||
|
||||
//---------------------------- mesh vs sdf broad phase --------------------------------------------
|
||||
|
||||
// tree vs sdf binary search
|
||||
static void collideBVH(const mjModel* m, mjData* d, int g,
|
||||
const mjtNum offset[3], const mjtNum rotation[9],
|
||||
int* faces, int* npoints, int* n0,
|
||||
const mjSDF* sdf) {
|
||||
const int bvhadr = m->mesh_bvhadr[m->geom_dataid[g]];
|
||||
const int* faceid = m->bvh_nodeid + bvhadr;
|
||||
const mjtNum* bvh = m->bvh_aabb + 6*bvhadr;
|
||||
const int* child = m->bvh_child + 2*bvhadr;
|
||||
mjtByte* bvh_active = m->vis.global.bvactive ? d->bvh_active + bvhadr : NULL;
|
||||
|
||||
mj_markStack(d);
|
||||
// TODO(quaglino): Store bvh max depths to make this bound tighter.
|
||||
int max_stack = m->mesh_bvhnum[m->geom_dataid[g]];
|
||||
struct CollideTreeArgs_ {
|
||||
int node;
|
||||
};
|
||||
typedef struct CollideTreeArgs_ CollideTreeArgs;
|
||||
CollideTreeArgs* stack = mjSTACKALLOC(d, max_stack, CollideTreeArgs);
|
||||
int nstack = 0;
|
||||
stack[nstack].node = 0;
|
||||
nstack++;
|
||||
|
||||
while (nstack) {
|
||||
(*n0)++;
|
||||
|
||||
// pop from stack
|
||||
nstack--;
|
||||
int node = stack[nstack].node;
|
||||
|
||||
// node1 is a leaf
|
||||
if (faceid[node] != -1) {
|
||||
if (boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) {
|
||||
faces[*npoints] = faceid[node];
|
||||
if (++(*npoints) == MAXSDFFACE) {
|
||||
mju_warning("mjc_MeshSDF: too many bounding volumes, some contacts may be missed");
|
||||
mj_freeStack(d);
|
||||
return;
|
||||
}
|
||||
if (bvh_active) bvh_active[node] = 1;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
// if no intersection at intermediate levels, stop
|
||||
if (!boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (bvh_active) bvh_active[node] = 1;
|
||||
|
||||
// recursive call
|
||||
for (int i=0; i < 2; i++) {
|
||||
if (child[2*node+i] != -1) {
|
||||
if (nstack >= max_stack) mjERROR("BVH stack depth exceeded.");
|
||||
stack[nstack].node = child[2*node+i];
|
||||
nstack++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
mj_freeStack(d);
|
||||
}
|
||||
|
||||
//------------------------------ collision functions -----------------------------------------------
|
||||
|
||||
@@ -760,88 +609,7 @@ int mjc_HFieldSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int
|
||||
|
||||
// collision between a mesh and a signed distance field
|
||||
int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
mjGETINFO;
|
||||
|
||||
mjtNum offset[3], rotation[9], corners[9], x[3], depth;
|
||||
mjtNum points[3*MAXSDFFACE], dist[MAXMESHPNT], candidate[3*MAXMESHPNT];
|
||||
int vertadr = m->mesh_vertadr[m->geom_dataid[g1]];
|
||||
int faceadr = m->mesh_faceadr[m->geom_dataid[g1]];
|
||||
int cnt=0, npoints=0, ncandidate=0, n0=0, faces[MAXSDFFACE]={-1}, index[MAXMESHPNT];
|
||||
|
||||
// get sdf plugin
|
||||
int instance = m->geom_plugin[g2];
|
||||
const mjpPlugin* sdf_ptr = instance == -1 ? NULL : mjc_getSDF(m, g2);
|
||||
instance = instance == -1 ? m->geom_dataid[g2] : instance;
|
||||
mjtGeom geomtype = mjGEOM_SDF;
|
||||
|
||||
// copy into data
|
||||
mjSDF sdf;
|
||||
sdf.id = &instance;
|
||||
sdf.type = mjSDFTYPE_SINGLE;
|
||||
sdf.plugin = &sdf_ptr;
|
||||
sdf.geomtype = &geomtype;
|
||||
|
||||
// compute transformation from g1 to g2
|
||||
mjtNum sdf_quat[4], quat1[4];
|
||||
mju_mat2Quat(quat1, mat1);
|
||||
mju_mat2Quat(sdf_quat, mat2);
|
||||
mapPose(pos1, quat1, pos2, sdf_quat, offset, rotation);
|
||||
|
||||
// binary tree search
|
||||
collideBVH(m, (mjData*)d, g1, offset, rotation, faces, &npoints, &n0, &sdf);
|
||||
|
||||
// Frank-Wolfe algorithm
|
||||
for (int i=0; i < npoints; i++) {
|
||||
int face = faceadr + faces[i];
|
||||
for (int v=0; v < 3; v++) {
|
||||
mjtNum vec[3] = {
|
||||
m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+0],
|
||||
m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+1],
|
||||
m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+2],
|
||||
};
|
||||
|
||||
// transform local 1 (mesh) to local 2 (sdf)
|
||||
mju_mulMatVec3(corners+3*v, rotation, vec);
|
||||
mju_addTo3(corners+3*v, offset);
|
||||
}
|
||||
|
||||
// stricter culling
|
||||
if (!triangleIntersect(corners, m, &sdf, (mjData*)d)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// starting point
|
||||
x[0] = (corners[0]+corners[3]+corners[6])/3;
|
||||
x[1] = (corners[1]+corners[4]+corners[7])/3;
|
||||
x[2] = (corners[2]+corners[5]+corners[8])/3;
|
||||
|
||||
// SHOULD NOT OCCUR
|
||||
if (ncandidate == MAXMESHPNT)mjERROR("too many contact points");
|
||||
|
||||
// Frank-Wolfe
|
||||
depth = stepFrankWolfe(x, corners, 3, m, &sdf, (mjData*)d);
|
||||
|
||||
// store candidate if there is penetration
|
||||
if (depth < 0) {
|
||||
mju_copy3(candidate + 3*ncandidate, x);
|
||||
index[ncandidate] = ncandidate;
|
||||
dist[ncandidate++] = depth;
|
||||
}
|
||||
}
|
||||
|
||||
// sort contacts using depth
|
||||
if (ncandidate > 1) {
|
||||
int buf[MAXMESHPNT];
|
||||
distSort(index, buf, ncandidate, dist);
|
||||
}
|
||||
|
||||
// add only the first mjMAXCONPAIR pairs
|
||||
for (int i=0; i < mju_min(ncandidate, mjMAXCONPAIR); i++) {
|
||||
cnt = addContact(points, con, candidate + 3*index[i], pos2, sdf_quat,
|
||||
dist[index[i]], cnt, m, &sdf, (mjData*)d);
|
||||
}
|
||||
|
||||
return cnt;
|
||||
return mjc_SDF(m, d, con, g1, g2, margin);
|
||||
}
|
||||
|
||||
// collision between two SDFs
|
||||
|
||||
Reference in New Issue
Block a user