|
| 1 | +/////////////////////////////////////////////////////////////////////////////// |
| 2 | +/// |
| 3 | +/// $Id$ |
| 4 | +/// |
| 5 | +/// Copyright (c) 1987- JSK, The University of Tokyo. All Rights Reserved. |
| 6 | +/// |
| 7 | +/// This software is a collection of EusLisp code for robot applications, |
| 8 | +/// which has been developed by the JSK Laboratory for the IRT project. |
| 9 | +/// For more information on EusLisp and its application to the robotics, |
| 10 | +/// please refer to the following papers. |
| 11 | +/// |
| 12 | +/// Toshihiro Matsui |
| 13 | +/// Multithread object-oriented language euslisp for parallel and |
| 14 | +/// asynchronous programming in robotics |
| 15 | +/// Workshop on Concurrent Object-based Systems, |
| 16 | +/// IEEE 6th Symposium on Parallel and Distributed Processing, 1994 |
| 17 | +/// |
| 18 | +/// Permission to use this software for educational, research |
| 19 | +/// and non-profit purposes, without fee, and without a written |
| 20 | +/// agreement is hereby granted to all researchers working on |
| 21 | +/// the IRT project at the University of Tokyo, provided that the |
| 22 | +/// above copyright notice remains intact. |
| 23 | +/// |
| 24 | + |
| 25 | +// for eus.h |
| 26 | +#include <stdio.h> |
| 27 | +#include <stdlib.h> |
| 28 | +#include <unistd.h> |
| 29 | +#include <string.h> |
| 30 | +#include <setjmp.h> |
| 31 | +#include <errno.h> |
| 32 | +#include <sstream> |
| 33 | + |
| 34 | +#define class eus_class |
| 35 | +#define throw eus_throw |
| 36 | +#define export eus_export |
| 37 | +#define vector eus_vector |
| 38 | +#define string eus_string |
| 39 | +#include <eus.h> // include eus.h just for eusfloat_t ... |
| 40 | +#undef class |
| 41 | +#undef throw |
| 42 | +#undef export |
| 43 | +#undef vector |
| 44 | +#undef string |
| 45 | + |
| 46 | +#if HAVE_BULLET |
| 47 | +#include <btBulletCollisionCommon.h> |
| 48 | +#include <BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h> |
| 49 | +#include <BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h> |
| 50 | +#include <BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h> |
| 51 | +#include <LinearMath/btGeometryUtil.h> |
| 52 | +#endif |
| 53 | + |
| 54 | +#if HAVE_BULLET |
| 55 | +#define CALL_WITH_BULLET_CHECK(X) X |
| 56 | +#else |
| 57 | +#define CALL_WITH_BULLET_CHECK(X) fprintf(stderr, "jskeus is compiled without bullet, so you can not use function %s. Please install bullet >= 2.83.\n", __PRETTY_FUNCTION__); return -1; |
| 58 | +#endif |
| 59 | + |
| 60 | + |
| 61 | +#if HAVE_BULLET |
| 62 | +struct btDistanceInfo |
| 63 | +{ // this class is copied from https://github.com/bulletphysics/bullet3/blob/master/test/collision/btDistanceInfo.h |
| 64 | + btVector3 m_pointOnA; |
| 65 | + btVector3 m_pointOnB; |
| 66 | + btVector3 m_normalBtoA; |
| 67 | + btScalar m_distance; |
| 68 | +}; |
| 69 | + |
| 70 | + |
| 71 | +struct ConvexWrap |
| 72 | +{ // this class is copied from https://github.com/bulletphysics/bullet3/blob/master/test/collision/main.cpp |
| 73 | + btConvexShape* m_convex; |
| 74 | + btTransform m_worldTrans; |
| 75 | + inline btScalar getMargin() const |
| 76 | + { |
| 77 | + return m_convex->getMargin(); |
| 78 | + } |
| 79 | + inline btVector3 getObjectCenterInWorld() const |
| 80 | + { |
| 81 | + return m_worldTrans.getOrigin(); |
| 82 | + } |
| 83 | + inline const btTransform& getWorldTransform() const |
| 84 | + { |
| 85 | + return m_worldTrans; |
| 86 | + } |
| 87 | + inline btVector3 getLocalSupportWithMargin(const btVector3& dir) const |
| 88 | + { |
| 89 | + return m_convex->localGetSupportingVertex(dir); |
| 90 | + } |
| 91 | + inline btVector3 getLocalSupportWithoutMargin(const btVector3& dir) const |
| 92 | + { |
| 93 | + return m_convex->localGetSupportingVertexWithoutMargin(dir); |
| 94 | + } |
| 95 | +}; |
| 96 | + |
| 97 | +long BT_MakeSphereModel(double radius) |
| 98 | +{ |
| 99 | + return (long)(new btSphereShape(radius)); |
| 100 | +}; |
| 101 | + |
| 102 | +long BT_MakeBoxModel(double xsize, double ysize, double zsize) |
| 103 | +{ |
| 104 | + return (long)(new btBoxShape(0.5*btVector3(xsize, ysize, zsize))); |
| 105 | +}; |
| 106 | + |
| 107 | +long BT_MakeCylinderModel(double radius, double height) |
| 108 | +{ |
| 109 | + return (long)(new btCylinderShapeZ(btVector3(radius, radius, 0.5*height))); |
| 110 | +}; |
| 111 | + |
| 112 | +long BT_MakeCapsuleModel(double radius, double height) |
| 113 | +{ |
| 114 | + return (long)(new btCapsuleShapeZ(radius, 0.5*height)); |
| 115 | +}; |
| 116 | + |
| 117 | +long BT_MakeMeshModel(double *verticesPoints, long numVertices) |
| 118 | +{ |
| 119 | + btConvexHullShape* pshape = new btConvexHullShape(); |
| 120 | +#define SHRINK_FOR_MARGIN false |
| 121 | + if (SHRINK_FOR_MARGIN) { |
| 122 | + // Shrink vertices for default margin CONVEX_DISTANCE_MARGIN, |
| 123 | + // which should be nonzero positive for fast computation of penetration distance. |
| 124 | + // ref: https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=2358#p9411 |
| 125 | + // But sometimes, this doesn't work well (vertices become empty), so currently disabled. |
| 126 | + btAlignedObjectArray<btVector3> vertices; |
| 127 | + for (int i = 0; i < 3 * numVertices; i += 3) { |
| 128 | + vertices.push_back(btVector3(verticesPoints[i], verticesPoints[i+1], verticesPoints[i+2])); |
| 129 | + } |
| 130 | + btAlignedObjectArray<btVector3> planes; |
| 131 | + btGeometryUtil::getPlaneEquationsFromVertices(vertices, planes); |
| 132 | + int sz = planes.size(); |
| 133 | + for (int i = 0 ; i < sz ; i++) { |
| 134 | + planes[i][3] += CONVEX_DISTANCE_MARGIN; |
| 135 | + } |
| 136 | + vertices.clear(); |
| 137 | + btGeometryUtil::getVerticesFromPlaneEquations(planes, vertices); |
| 138 | + sz = vertices.size(); |
| 139 | + for (int i = 0 ; i < sz ; i++) { |
| 140 | + pshape->addPoint(vertices[i]); |
| 141 | + } |
| 142 | + } else { |
| 143 | + for (int i = 0; i < 3 * numVertices; i += 3) { |
| 144 | + pshape->addPoint(btVector3(verticesPoints[i], verticesPoints[i+1], verticesPoints[i+2])); |
| 145 | + } |
| 146 | + } |
| 147 | + return (long)pshape; |
| 148 | +}; |
| 149 | + |
| 150 | +long BT_CalcCollisionDistance(long modelAddrA, long modelAddrB, |
| 151 | + double *posA, double *quatA, double *posB, double *quatB, |
| 152 | + double *dist, double *dir, double *pA, double *pB) |
| 153 | +{ |
| 154 | + ConvexWrap a, b; |
| 155 | + a.m_convex = ((btConvexShape *)modelAddrA); |
| 156 | + a.m_worldTrans.setOrigin(btVector3(posA[0], posA[1], posA[2])); |
| 157 | + a.m_worldTrans.setRotation(btQuaternion(quatA[1], quatA[2], quatA[3], quatA[0])); // w is first element in euslisp |
| 158 | + b.m_convex = ((btConvexShape *)modelAddrB); |
| 159 | + b.m_worldTrans.setOrigin(btVector3(posB[0], posB[1], posB[2])); |
| 160 | + b.m_worldTrans.setRotation(btQuaternion(quatB[1], quatB[2], quatB[3], quatB[0])); // w is first element in euslisp |
| 161 | + // The origin of euslisp cylinder model is located on bottom, so local translation of half height is necessary |
| 162 | + if(btCylinderShapeZ* cly = dynamic_cast<btCylinderShapeZ*>(a.m_convex)) { |
| 163 | + btVector3 heightOffset(btVector3(0, 0, cly->getHalfExtentsWithMargin().getZ())); |
| 164 | + a.m_worldTrans.setOrigin(a.m_worldTrans.getOrigin() + a.m_worldTrans.getBasis() * heightOffset); |
| 165 | + } |
| 166 | + if(btCylinderShapeZ* cly = dynamic_cast<btCylinderShapeZ*>(b.m_convex)) { |
| 167 | + btVector3 heightOffset(btVector3(0, 0, cly->getHalfExtentsWithMargin().getZ())); |
| 168 | + b.m_worldTrans.setOrigin(b.m_worldTrans.getOrigin() + b.m_worldTrans.getBasis() * heightOffset); |
| 169 | + } |
| 170 | + |
| 171 | + btGjkCollisionDescription colDesc; |
| 172 | + btVoronoiSimplexSolver simplexSolver; |
| 173 | + btDistanceInfo distInfo; |
| 174 | + int res = -1; |
| 175 | + simplexSolver.reset(); |
| 176 | + res = btComputeGjkEpaPenetration(a, b, colDesc, simplexSolver, &distInfo); |
| 177 | + |
| 178 | + // The result of btComputeGjkEpaPenetration is offseted by CONVEX_DISTANCE_MARGIN. |
| 179 | + // Although the offset is considered internally in primitive shapes, not considered in convex hull shape. |
| 180 | + // So, the result is modified manually. |
| 181 | + if(dynamic_cast<btConvexHullShape*>((btConvexShape *)modelAddrA)) { |
| 182 | + distInfo.m_distance += CONVEX_DISTANCE_MARGIN; |
| 183 | + distInfo.m_pointOnA += CONVEX_DISTANCE_MARGIN * distInfo.m_normalBtoA; |
| 184 | + } |
| 185 | + if(dynamic_cast<btConvexHullShape*>((btConvexShape *)modelAddrB)) { |
| 186 | + distInfo.m_distance += CONVEX_DISTANCE_MARGIN; |
| 187 | + distInfo.m_pointOnB += - CONVEX_DISTANCE_MARGIN * distInfo.m_normalBtoA; |
| 188 | + } |
| 189 | + |
| 190 | + *dist = distInfo.m_distance; |
| 191 | + for (int i = 0; i < 3; i++) { |
| 192 | + dir[i] = distInfo.m_normalBtoA[i]; |
| 193 | + pA[i] = distInfo.m_pointOnA[i]; |
| 194 | + pB[i] = distInfo.m_pointOnB[i]; |
| 195 | + } |
| 196 | + |
| 197 | + return res; |
| 198 | +}; |
| 199 | + |
| 200 | +long BT_SetMargin(long modelAddr, double margin) |
| 201 | +{ |
| 202 | + // shape are shrinked for CONVEX_DISTANCE_MARGIN, so CONVEX_DISTANCE_MARGIN is added to margin |
| 203 | + ((btConvexShape *)modelAddr)->setMargin(CONVEX_DISTANCE_MARGIN+margin); |
| 204 | + return 0; |
| 205 | +}; |
| 206 | +#endif |
| 207 | + |
| 208 | +extern "C" |
| 209 | +{ |
| 210 | + eusinteger_t C_BT_MakeSphereModel(eusfloat_t r) |
| 211 | + { |
| 212 | + CALL_WITH_BULLET_CHECK(return BT_MakeSphereModel(r);) |
| 213 | + } |
| 214 | + |
| 215 | + eusinteger_t C_BT_MakeBoxModel(eusfloat_t xsize, eusfloat_t ysize, eusfloat_t zsize) |
| 216 | + { |
| 217 | + CALL_WITH_BULLET_CHECK(return BT_MakeBoxModel(xsize, ysize, zsize);) |
| 218 | + } |
| 219 | + |
| 220 | + eusinteger_t C_BT_MakeCylinderModel(eusfloat_t radius, eusfloat_t height) |
| 221 | + { |
| 222 | + CALL_WITH_BULLET_CHECK(return BT_MakeCylinderModel(radius, height);) |
| 223 | + } |
| 224 | + |
| 225 | + eusinteger_t C_BT_MakeCapsuleModel(eusfloat_t radius, eusfloat_t height) |
| 226 | + { |
| 227 | + CALL_WITH_BULLET_CHECK(return BT_MakeCapsuleModel(radius, height);) |
| 228 | + } |
| 229 | + |
| 230 | + eusinteger_t C_BT_MakeMeshModel(eusfloat_t *verticesPoints, eusinteger_t numVertices) |
| 231 | + { |
| 232 | + #if HAVE_BULLET |
| 233 | + double _verticesPoints[3*numVertices]; |
| 234 | + for (int i = 0; i < 3 * numVertices; i++ ) { _verticesPoints[i] = verticesPoints[i]; } |
| 235 | + #endif |
| 236 | + CALL_WITH_BULLET_CHECK(return BT_MakeMeshModel(_verticesPoints, numVertices);) |
| 237 | + } |
| 238 | + |
| 239 | + eusinteger_t C_BT_CalcCollisionDistance(eusinteger_t modelAddrA, eusinteger_t modelAddrB, |
| 240 | + eusfloat_t *posA, eusfloat_t *quatA, eusfloat_t *posB, eusfloat_t *quatB, |
| 241 | + eusfloat_t *dist, eusfloat_t *dir, eusfloat_t *pA, eusfloat_t *pB) |
| 242 | + { |
| 243 | + #if HAVE_BULLET |
| 244 | + double _posA[3], _quatA[4], _posB[3], _quatB[4]; |
| 245 | + double _dist[1], _dir[3], _pA[3], _pB[3]; |
| 246 | + eusinteger_t ret; |
| 247 | + for (int i = 0; i < 3; i++ ) {_posA[i] = posA[i]; _posB[i] = posB[i]; } |
| 248 | + for (int i = 0; i < 4; i++ ) {_quatA[i] = quatA[i]; _quatB[i] = quatB[i]; } |
| 249 | + _dist[0] = dist[0]; |
| 250 | + for (int i = 0; i < 3; i++ ) {_dir[i] = dir[i]; _pA[i] = pA[i]; _pB[i] = pB[i];} |
| 251 | + ret = BT_CalcCollisionDistance(modelAddrA, modelAddrB, |
| 252 | + _posA, _quatA, _posB, _quatB, |
| 253 | + _dist, _dir, _pA, _pB); |
| 254 | + dist[0] = _dist[0]; |
| 255 | + for (int i = 0; i < 3; i++ ) {dir[i] = _dir[i]; pA[i] = _pA[i]; pB[i] = _pB[i];} |
| 256 | + #endif |
| 257 | + CALL_WITH_BULLET_CHECK(return ret;) |
| 258 | + } |
| 259 | + |
| 260 | + eusinteger_t C_BT_SetMargin(eusinteger_t modelAddr, eusfloat_t margin) |
| 261 | + { |
| 262 | + CALL_WITH_BULLET_CHECK(return BT_SetMargin(modelAddr, margin);) |
| 263 | + } |
| 264 | +} |
0 commit comments