Add eusbullet for using bullet collision function #538
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,265 @@ | ||
| /////////////////////////////////////////////////////////////////////////////// | ||
| /// | ||
| /// $Id$ | ||
| /// | ||
| /// Copyright (c) 1987- JSK, The University of Tokyo. All Rights Reserved. | ||
| /// | ||
| /// This software is a collection of EusLisp code for robot applications, | ||
| /// which has been developed by the JSK Laboratory for the IRT project. | ||
| /// For more information on EusLisp and it's application to the robotics, | ||
| /// please refer to the following papers. | ||
| /// | ||
| /// Toshihiro Matsui | ||
| /// Multithread object-oriented language euslisp for parallel and | ||
| /// asynchronous programming in robotics | ||
| /// Workshop on Concurrent Object-based Systems, | ||
| /// IEEE 6th Symposium on Parallel and Distributed Processing, 1994 | ||
| /// | ||
| /// Permission to use this software for educational, research | ||
| /// and non-profit purposes, without fee, and without a written | ||
| /// agreement is hereby granted to all researchers working on | ||
| /// the IRT project at the University of Tokyo, provided that the | ||
| /// above copyright notice remains intact. | ||
| /// | ||
|
|
||
|
|
||
| // for eus.h | ||
| #include <stdio.h> | ||
| #include <stdlib.h> | ||
| #include <unistd.h> | ||
| #include <string.h> | ||
| #include <setjmp.h> | ||
| #include <errno.h> | ||
| #include <sstream> | ||
|
|
||
| #define class eus_class | ||
| #define throw eus_throw | ||
| #define export eus_export | ||
| #define vector eus_vector | ||
| #define string eus_string | ||
| #include <eus.h> // include eus.h just for eusfloat_t ... | ||
| #undef class | ||
| #undef throw | ||
| #undef export | ||
| #undef vector | ||
| #undef string | ||
|
|
||
|
|
||
| #if HAVE_BULLET | ||
| #include <btBulletCollisionCommon.h> | ||
| #include <BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h> | ||
| #include <BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h> | ||
| #include <BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h> | ||
| #include <LinearMath/btGeometryUtil.h> | ||
| #endif | ||
|
|
||
| #if HAVE_BULLET | ||
| #define CALL_WITH_BULLET_CHECK(X) X | ||
| #else | ||
| #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; | ||
| #endif | ||
|
|
||
|
|
||
| #if HAVE_BULLET | ||
| struct btDistanceInfo | ||
| { // this class is copied from https://github.com/bulletphysics/bullet3/blob/master/test/collision/btDistanceInfo.h | ||
| btVector3 m_pointOnA; | ||
| btVector3 m_pointOnB; | ||
| btVector3 m_normalBtoA; | ||
| btScalar m_distance; | ||
| }; | ||
|
|
||
|
|
||
| struct ConvexWrap | ||
| { // this class is copied from https://github.com/bulletphysics/bullet3/blob/master/test/collision/main.cpp | ||
| btConvexShape* m_convex; | ||
| btTransform m_worldTrans; | ||
| inline btScalar getMargin() const | ||
| { | ||
| return m_convex->getMargin(); | ||
| } | ||
| inline btVector3 getObjectCenterInWorld() const | ||
| { | ||
| return m_worldTrans.getOrigin(); | ||
| } | ||
| inline const btTransform& getWorldTransform() const | ||
| { | ||
| return m_worldTrans; | ||
| } | ||
| inline btVector3 getLocalSupportWithMargin(const btVector3& dir) const | ||
| { | ||
| return m_convex->localGetSupportingVertex(dir); | ||
| } | ||
| inline btVector3 getLocalSupportWithoutMargin(const btVector3& dir) const | ||
| { | ||
| return m_convex->localGetSupportingVertexWithoutMargin(dir); | ||
| } | ||
| }; | ||
|
|
||
| long makeSphereModel(double radius) | ||
| { | ||
| return (long)(new btSphereShape(radius)); | ||
| }; | ||
|
|
||
| long makeBoxModel(double xsize, double ysize, double zsize) | ||
| { | ||
| return (long)(new btBoxShape(0.5*btVector3(xsize, ysize, zsize))); | ||
| }; | ||
|
|
||
| long makeCylinderModel(double radius, double height) | ||
| { | ||
| return (long)(new btCylinderShapeZ(btVector3(radius, radius, 0.5*height))); | ||
| }; | ||
|
|
||
| long makeCapsuleModel(double radius, double height) | ||
| { | ||
| return (long)(new btCapsuleShapeZ(radius, 0.5*height)); | ||
| }; | ||
|
|
||
| long makeMeshModel(double *verticesPoints, long numVertices) | ||
| { | ||
| btConvexHullShape* pshape = new btConvexHullShape(); | ||
| #define SHRINK_FOR_MARGIN false | ||
| if (SHRINK_FOR_MARGIN) { | ||
| // Shrink vertices for default margin CONVEX_DISTANCE_MARGIN, | ||
| // which should be nonzero positive for fast computation of penetration distance. | ||
| // ref: https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=2358#p9411 | ||
| // But sometimes, this doesn't work well (vertices become empty), so currently disabled. | ||
| btAlignedObjectArray<btVector3> vertices; | ||
| for (int i = 0; i < 3 * numVertices; i += 3) { | ||
| vertices.push_back(btVector3(verticesPoints[i], verticesPoints[i+1], verticesPoints[i+2])); | ||
| } | ||
| btAlignedObjectArray<btVector3> planes; | ||
| btGeometryUtil::getPlaneEquationsFromVertices(vertices, planes); | ||
| int sz = planes.size(); | ||
| for (int i = 0 ; i < sz ; i++) { | ||
| planes[i][3] += CONVEX_DISTANCE_MARGIN; | ||
| } | ||
| vertices.clear(); | ||
| btGeometryUtil::getVerticesFromPlaneEquations(planes, vertices); | ||
| sz = vertices.size(); | ||
| for (int i = 0 ; i < sz ; i++) { | ||
| pshape->addPoint(vertices[i]); | ||
| } | ||
| } else { | ||
| for (int i = 0; i < 3 * numVertices; i += 3) { | ||
| pshape->addPoint(btVector3(verticesPoints[i], verticesPoints[i+1], verticesPoints[i+2])); | ||
| } | ||
| } | ||
| return (long)pshape; | ||
| }; | ||
|
|
||
| long calcCollisionDistance(long modelAddrA, long modelAddrB, | ||
| double *posA, double *quatA, double *posB, double *quatB, | ||
| double *dist, double *dir, double *pA, double *pB) | ||
| { | ||
| ConvexWrap a, b; | ||
| a.m_convex = ((btConvexShape *)modelAddrA); | ||
| a.m_worldTrans.setOrigin(btVector3(posA[0], posA[1], posA[2])); | ||
| a.m_worldTrans.setRotation(btQuaternion(quatA[1], quatA[2], quatA[3], quatA[0])); // w is first element in euslisp | ||
| b.m_convex = ((btConvexShape *)modelAddrB); | ||
| b.m_worldTrans.setOrigin(btVector3(posB[0], posB[1], posB[2])); | ||
| b.m_worldTrans.setRotation(btQuaternion(quatB[1], quatB[2], quatB[3], quatB[0])); // w is first element in euslisp | ||
| // The origin of euslisp cylinder model is located on bottom, so local translation of half height is necessary | ||
| if(btCylinderShapeZ* cly = dynamic_cast<btCylinderShapeZ*>(a.m_convex)) { | ||
| btVector3 heightOffset(btVector3(0, 0, cly->getHalfExtentsWithMargin().getZ())); | ||
| a.m_worldTrans.setOrigin(a.m_worldTrans.getOrigin() + a.m_worldTrans.getBasis() * heightOffset); | ||
| } | ||
| if(btCylinderShapeZ* cly = dynamic_cast<btCylinderShapeZ*>(b.m_convex)) { | ||
| btVector3 heightOffset(btVector3(0, 0, cly->getHalfExtentsWithMargin().getZ())); | ||
| b.m_worldTrans.setOrigin(b.m_worldTrans.getOrigin() + b.m_worldTrans.getBasis() * heightOffset); | ||
| } | ||
|
|
||
| btGjkCollisionDescription colDesc; | ||
| btVoronoiSimplexSolver simplexSolver; | ||
| btDistanceInfo distInfo; | ||
| int res = -1; | ||
| simplexSolver.reset(); | ||
| res = btComputeGjkEpaPenetration(a, b, colDesc, simplexSolver, &distInfo); | ||
|
|
||
| // The result of btComputeGjkEpaPenetration is offseted by CONVEX_DISTANCE_MARGIN. | ||
| // Although the offset is considered internally in primitive shapes, not considered in convex hull shape. | ||
| // So, the result is modified manually. | ||
| if(dynamic_cast<btConvexHullShape*>((btConvexShape *)modelAddrA)) { | ||
| distInfo.m_distance += CONVEX_DISTANCE_MARGIN; | ||
| distInfo.m_pointOnA += CONVEX_DISTANCE_MARGIN * distInfo.m_normalBtoA; | ||
| } | ||
| if(dynamic_cast<btConvexHullShape*>((btConvexShape *)modelAddrB)) { | ||
| distInfo.m_distance += CONVEX_DISTANCE_MARGIN; | ||
| distInfo.m_pointOnB += - CONVEX_DISTANCE_MARGIN * distInfo.m_normalBtoA; | ||
| } | ||
|
|
||
| *dist = distInfo.m_distance; | ||
| for (int i = 0; i < 3; i++) { | ||
| dir[i] = distInfo.m_normalBtoA[i]; | ||
| pA[i] = distInfo.m_pointOnA[i]; | ||
| pB[i] = distInfo.m_pointOnB[i]; | ||
| } | ||
|
|
||
| return res; | ||
| }; | ||
|
|
||
| long setMargin(long modelAddr, double margin) | ||
| { | ||
| // shape are shrinked for CONVEX_DISTANCE_MARGIN, so CONVEX_DISTANCE_MARGIN is added to margin | ||
| ((btConvexShape *)modelAddr)->setMargin(CONVEX_DISTANCE_MARGIN+margin); | ||
| return 0; | ||
| }; | ||
| #endif | ||
|
|
||
| extern "C" { | ||
| eusinteger_t callMakeSphereModel(eusfloat_t *r) | ||
| { | ||
| CALL_WITH_BULLET_CHECK(return makeSphereModel(r[0]);) | ||
| } | ||
|
|
||
| eusinteger_t callMakeBoxModel(eusfloat_t *xyz) | ||
| { | ||
| CALL_WITH_BULLET_CHECK(return makeBoxModel(xyz[0], xyz[1], xyz[2]);) | ||
| } | ||
|
|
||
| eusinteger_t callMakeCylinderModel(eusfloat_t *rh) | ||
| { | ||
| CALL_WITH_BULLET_CHECK(return makeCylinderModel(rh[0], rh[1]);) | ||
| } | ||
|
|
||
| eusinteger_t callMakeCapsuleModel(eusfloat_t *rh) | ||
| { | ||
| CALL_WITH_BULLET_CHECK(return makeCapsuleModel(rh[0], rh[1]);) | ||
| } | ||
|
|
||
| eusinteger_t callMakeMeshModel(eusfloat_t *verticesPoints, eusinteger_t numVertices) | ||
| { | ||
| #if HAVE_BULLET | ||
| double _verticesPoints[3*numVertices]; | ||
| for (int i = 0; i < 3 * numVertices; i++ ) { _verticesPoints[i] = verticesPoints[i]; } | ||
| #endif | ||
| CALL_WITH_BULLET_CHECK(return makeMeshModel(_verticesPoints, numVertices);) | ||
| } | ||
|
|
||
| eusinteger_t callCalcCollisionDistance(eusinteger_t modelAddrA, eusinteger_t modelAddrB, | ||
| eusfloat_t *posA, eusfloat_t *quatA, eusfloat_t *posB, eusfloat_t *quatB, | ||
| eusfloat_t *dist, eusfloat_t *dir, eusfloat_t *pA, eusfloat_t *pB) | ||
| { | ||
| #if HAVE_BULLET | ||
| double _posA[3], _quatA[4], _posB[3], _quatB[4]; | ||
| double _dist[1], _dir[3], _pA[3], _pB[3]; | ||
| eusinteger_t ret; | ||
| for (int i = 0; i < 3; i++ ) {_posA[i] = posA[i]; _posB[i] = posB[i]; } | ||
| for (int i = 0; i < 4; i++ ) {_quatA[i] = quatA[i]; _quatB[i] = quatB[i]; } | ||
| _dist[0] = dist[0]; | ||
| for (int i = 0; i < 3; i++ ) {_dir[i] = dir[i]; _pA[i] = pA[i]; _pB[i] = pB[i];} | ||
| ret = calcCollisionDistance(modelAddrA, modelAddrB, | ||
| _posA, _quatA, _posB, _quatB, | ||
| _dist, _dir, _pA, _pB); | ||
| dist[0] = _dist[0]; | ||
| for (int i = 0; i < 3; i++ ) {dir[i] = _dir[i]; pA[i] = _pA[i]; pB[i] = _pB[i];} | ||
| #endif | ||
| CALL_WITH_BULLET_CHECK(return ret;) | ||
| } | ||
|
|
||
| eusinteger_t callSetMargin(eusinteger_t modelAddr, eusfloat_t *margin) | ||
| { | ||
| CALL_WITH_BULLET_CHECK(return setMargin(modelAddr, margin[0]);) | ||
| } | ||
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,33 @@ | ||
| include Makefile.$(ARCHDIR) | ||
|
There was a problem hiding this comment.
Otherwise, I think we'd better to create please also consider creating and add something like please also remove 'eusbullet' from Please consider updating https://jskeus.readthedocs.io/ja/latest/pqp.html, in the first section (call PQP from irteus) is mainly explaining implementation so you can put the end of the document and that of bullet library is not necessary. update test code should be run on both pqp/bullet and also we'd like to check for cube/matrix cases. |
||
|
|
||
| CFLAGS = -I. -I.. -D$(ARCHDIR) -D$(MACHINE) -DHAVE_BULLET=$(HAVE_BULLET) `pkg-config bullet --cflags` | ||
| LDFLAGS = -L. -L.. `pkg-config bullet --libs` | ||
|
|
||
| .SUFFIXES: .cpp | ||
|
|
||
| SRCS = BulletCollision.cpp | ||
|
|
||
| OBJECTS = $(ARCHDIR)/BulletCollision.o | ||
|
|
||
| CLEAN = $(ARCHDIR)/$(LPFX)BULLET.$(LSFX) $(OBJECTS) *.$(LSFX) | ||
|
|
||
| default: directory library | ||
|
|
||
| directory: | ||
| if [ ! -e $(ARCHDIR) ]; then mkdir -p $(ARCHDIR); fi | ||
|
|
||
| install: $(EUSDIR)/$(ARCHDIR)/lib/$(LPFX)BULLET.$(LSFX) | ||
|
|
||
| $(EUSDIR)/$(ARCHDIR)/lib/$(LPFX)BULLET.$(LSFX): $(ARCHDIR)/$(LPFX)BULLET.$(LSFX) | ||
| cp $(ARCHDIR)/$(LPFX)BULLET.$(LSFX) $(EUSDIR)/$(ARCHDIR)/lib | ||
|
|
||
| library: $(ARCHDIR)/$(LPFX)BULLET.$(LSFX) | ||
|
|
||
| $(ARCHDIR)/$(LPFX)BULLET.$(LSFX): $(OBJECTS) | ||
| $(LD) $(SOFLAGS) $(OUTOPT)$(ARCHDIR)/$(LPFX)BULLET.$(LSFX) $(OBJECTS) $(LDFLAGS) | ||
|
|
||
| $(OBJECTS): $(SRCS) | ||
| $(CC) $(CFLAGS) -DCOMPILE_LIB -c $(*F).cpp $(OBJOPT)$@ | ||
|
|
||
| clean: | ||
| rm -f $(CLEAN) | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,10 @@ | ||
| CC = c++ -O2 -falign-functions=4 | ||
| OBJOPT = -o | ||
| OUTOPT = -o | ||
| LD = c++ -shared -falign-functions=4 | ||
| EXELD = c++ -falign-functions=4 | ||
| SOFLAGS = | ||
| EXESFX = .exe | ||
| LSFX = dll | ||
| LPFX = lib | ||
| LIBS = -L$(ARCHDIR) -lRAPID |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,10 @@ | ||
| CC = c++ -O2 -falign-functions=8 -fPIC -DDarwin -DGCC -I$(EUSDIR)/include | ||
| OBJOPT = -o | ||
| OUTOPT = -o | ||
| LD = c++ | ||
| SOFLAGS = -dynamiclib -flat_namespace -undefined suppress | ||
| EXELD = c++ | ||
| EXESFX = | ||
| LSFX = so | ||
| LPFX = lib | ||
| LIBS = -L$(ARCHDIR) -lRAPID |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,19 @@ | ||
| CC = c++ -O2 -DLinux -DGCC -I$(EUSDIR)/include | ||
| OBJOPT = -o | ||
| OUTOPT = -o | ||
| LD = c++ | ||
| SOFLAGS = -shared | ||
| EXELD = c++ | ||
| EXESFX = | ||
| LSFX = so | ||
| LPFX = lib | ||
| LIBS = -L$(ARCHDIR) -lRAPID | ||
|
|
||
|
|
||
| ifeq ($(shell /bin/uname -m), x86_64) | ||
| CC += -m32 | ||
| LD += -m32 | ||
| EXELD += -m32 | ||
| endif | ||
|
There was a problem hiding this comment. currently we do not check There was a problem hiding this comment. As far as I know, we do not need intel 32bit. This |
||
|
|
||
|
|
||
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
please also update README.md
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Updated: 7aa804f