diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
index 885e67479f44d96701dbd7e35a175dcdf7f53f43..1b63fa7c392a7cc3543e6745586489b988291ff7 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
@@ -52,13 +52,11 @@ namespace SimDynamics
{
std::vector<Primitive::PrimitivePtr> primitives = colModel->getVisualization()->primitives;
- /*if (primitives.size() == 1 && primitives[0]->transform.isIdentity())
- {
- collisionShape.reset(getShapeFromPrimitive(primitives[0]));
- } else
- */
if (primitives.size() > 0)
{
+ //cout << "Object:" << o->getName() << endl;
+ //o->print();
+
btCompoundShape* compoundShape = new btCompoundShape(true);
std::vector<Primitive::PrimitivePtr>::const_iterator it;
Eigen::Matrix4f currentTransform = Eigen::Matrix4f::Identity();
@@ -66,17 +64,27 @@ namespace SimDynamics
Eigen::Matrix4f localComTransform;
localComTransform.setIdentity();
localComTransform.block(0,3,3,1) = -o->getCoMLocal();
- //cout << "localComTransform:\n" << localComTransform;
+ //cout << "localComTransform:\n" << localComTransform << endl;
for (it = primitives.begin(); it != primitives.end(); it++)
{
//currentTransform *= (*it)->transform;
currentTransform = localComTransform*(*it)->transform;
+ //cout << "primitive: (*it)->transform:\n" << (*it)->transform << endl;
+ //cout << "primitive: currentTransform:\n" << currentTransform << endl;
+
compoundShape->addChildShape(BulletEngine::getPoseBullet(currentTransform), getShapeFromPrimitive(*it));
}
collisionShape.reset(compoundShape);
+
+ // update com
+ Eigen::Matrix4f comLoc;
+ comLoc.setIdentity();
+ comLoc.block(0, 3, 3, 1) = o->getCoMGlobal();
+ comLoc = (o->getGlobalPose().inverse() * comLoc);
+ com = comLoc.block(0, 3, 3, 1);
}
else
{
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp
index 5672483fa924b94caf149358a6eb68f463a30910..ac701b8b69a554ef05a47b9b8cf819600b045d22 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp
@@ -6,6 +6,7 @@ namespace SimDynamics
BulletOpenGLViewer::BulletOpenGLViewer(DynamicsWorldPtr world)
{
+ this->world = world;
SIMDYNAMICS_ASSERT(world);
m_sundirection = btVector3(1, 1, -2) * 1000;
@@ -77,15 +78,15 @@ namespace SimDynamics
void BulletOpenGLViewer::keyboardCallback(unsigned char key, int x, int y)
{
- DemoApplication::keyboardCallback(key, x, y);
- /*switch (key)
+ //DemoApplication::keyboardCallback(key, x, y);
+ switch (key)
{
- case 'e':
- spawnRagdoll(true);
+ case 'p':
+ bulletEngine->print();
break;
default:
DemoApplication::keyboardCallback(key, x, y);
- }*/
+ }
}
BulletOpenGLViewer::~BulletOpenGLViewer()
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.h
index 875fab1b90c46fc4fd39ba022cdebd29db71faf7..a07be7fee1a66ae7e0128b8f1962ae9d040db0d9 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.h
@@ -73,6 +73,8 @@ namespace SimDynamics
return demo;
}*/
+ DynamicsWorldPtr world;
+
void updateRobotConstraints();
GLDebugDrawer debugDrawer;
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index 3e98dac009c14590fd7dabce5ce52c6e6941331d..35b99eb69ec13aaecff0368b54da78ee499f318e 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -640,6 +640,9 @@ namespace SimDynamics
{
const SimDynamics::DynamicsEngine::DynamicsContactInfo& contact = *it;
+ if (!contact.objectA || !contact.objectB)
+ continue;
+
float sign;
std::string key;
std::string contactBody;
diff --git a/SimDynamics/DynamicsWorld.h b/SimDynamics/DynamicsWorld.h
index 8968f9f36c58773e4f8f76cd31cf9d5358ac5553..b247cd1fe53a192e15667962e34845b63bc558c4 100644
--- a/SimDynamics/DynamicsWorld.h
+++ b/SimDynamics/DynamicsWorld.h
@@ -58,7 +58,7 @@ namespace SimDynamics
/*!
Build a dynamic version of your VirtualRobot::SceneObject. This can be an Obstacle or a ManipulationObject.
Internally the value of o->getSimulationtype() is queried in order to specify which type of simulation should be performed.
- If the simualtion type is not specified a full dynamic object is created (as with eDynamic).
+ If the simulation type is not specified a full dynamic object is created (as with eDynamic).
*/
static DynamicsObjectPtr CreateDynamicsObject(VirtualRobot::SceneObjectPtr o);
diff --git a/SimDynamics/examples/BulletDebugViewer/BulletDebugViewerGlut.cpp b/SimDynamics/examples/BulletDebugViewer/BulletDebugViewerGlut.cpp
index a5f2bcc1a308e527e774f3f94304eb508ad4f067..8a31e50c7ec2bc0437a8c10274af1d71f2c0db40 100644
--- a/SimDynamics/examples/BulletDebugViewer/BulletDebugViewerGlut.cpp
+++ b/SimDynamics/examples/BulletDebugViewer/BulletDebugViewerGlut.cpp
@@ -160,20 +160,35 @@ int main(int argc, char* argv[])
world->createFloorPlane();
VirtualRobot::ObstaclePtr o = VirtualRobot::Obstacle::createBox(100.0f, 100.0f, 100.0f);
+ o->setSimulationType(SceneObject::Physics::eDynamic);
+ Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
+ gp.block(0,3,3,1) = Eigen::Vector3f(2800, 10400, 5000.0f);
+ o->setGlobalPose(gp);
o->setMass(1.0f); // 1kg
SimDynamics::DynamicsObjectPtr dynObj = world->CreateDynamicsObject(o);
- dynObj->setPosition(Eigen::Vector3f(3000, 3000, 1000.0f));
world->addObject(dynObj);
+ o->print();
+
#if 0
std::string f = "/home/niko/coding/armarx/SimulationX/data/environment/KIT_Robot_Kitchen.xml";
ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(f);
SimDynamics::DynamicsObjectPtr dynObj2 = world->CreateDynamicsObject(mo, DynamicsObject::eKinematic);
+ world->addObject(dynObj2);
+#endif
+
+
+#if 0
+ std::string f = "/home/SMBAD/vahrenka/.armarx/mongo/.cache/files/lowersink.xml";
+ ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(f);
+ SimDynamics::DynamicsObjectPtr dynObj2 = world->CreateDynamicsObject(mo);
//dynObj->setPosition(Eigen::Vector3f(3000,3000,1000.0f));
world->addObject(dynObj2);
#endif
+
+
VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFile);
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robFile);
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
index 095b5be20ddbe81dbd53ccfe98efbb1413e70211..b8916a0e7cf16128d27c86f43404386feef9efce 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
@@ -82,6 +82,17 @@ SimDynamicsWindow::SimDynamicsWindow(std::string& sRobotFilename, Qt::WFlags fla
dynamicsWorld->addObject(dynamicsObject2);
#endif
+#if 0
+ std::string f = "/home/SMBAD/vahrenka/.armarx/mongo/.cache/files/lowersink.xml";
+ ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(f);
+ mo->setSimulationType(SceneObject::Physics::eDynamic);
+ SimDynamics::DynamicsObjectPtr dynObj2 = dynamicsWorld->CreateDynamicsObject(mo);
+ //dynObj2->setSimType(VirtualRobot::SceneObject::Physics::eDynamic);
+ dynObj2->setPosition(Eigen::Vector3f(0,0,-200.0f));
+ dynamicsWorld->addObject(dynObj2);
+ dynamicsObjects.push_back(dynObj2);
+#endif
+
setupUI();
loadRobot(robotFilename);