diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
index 2d65b6d053087b0f71d0732ad71afa76e943c7db..fea0ceffb9781d06a34489ce448a740d722ceef5 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
@@ -27,7 +27,7 @@ namespace SimDynamics {
BulletObject::BulletObject(VirtualRobot::SceneObjectPtr o)
: DynamicsObject(o)
{
- double interatiaFactor = 1.0f;
+ btScalar interatiaFactor = btScalar(1.0);
#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
interatiaFactor = 5.0f;
#endif
@@ -185,9 +185,9 @@ btConvexHullShape* BulletObject::createConvexHullShape(VirtualRobot::TriMeshMode
sc = 0.001f;
for(size_t i = 0; i < trimesh->faces.size(); i++) {
- btVector3 v1((trimesh->vertices[trimesh->faces[i].id1][0]-com[0])*sc,(trimesh->vertices[trimesh->faces[i].id1][1]-com[1])*sc,(trimesh->vertices[trimesh->faces[i].id1][2]-com[2])*sc);
- btVector3 v2((trimesh->vertices[trimesh->faces[i].id2][0]-com[0])*sc,(trimesh->vertices[trimesh->faces[i].id2][1]-com[1])*sc,(trimesh->vertices[trimesh->faces[i].id2][2]-com[2])*sc);
- btVector3 v3((trimesh->vertices[trimesh->faces[i].id3][0]-com[0])*sc,(trimesh->vertices[trimesh->faces[i].id3][1]-com[1])*sc,(trimesh->vertices[trimesh->faces[i].id3][2]-com[2])*sc);
+ btVector3 v1(btScalar((trimesh->vertices[trimesh->faces[i].id1][0]-com[0])*sc),btScalar((trimesh->vertices[trimesh->faces[i].id1][1]-com[1])*sc),btScalar((trimesh->vertices[trimesh->faces[i].id1][2]-com[2])*sc));
+ btVector3 v2(btScalar((trimesh->vertices[trimesh->faces[i].id2][0]-com[0])*sc),btScalar((trimesh->vertices[trimesh->faces[i].id2][1]-com[1])*sc),btScalar((trimesh->vertices[trimesh->faces[i].id2][2]-com[2])*sc));
+ btVector3 v3(btScalar((trimesh->vertices[trimesh->faces[i].id3][0]-com[0])*sc),btScalar((trimesh->vertices[trimesh->faces[i].id3][1]-com[1])*sc),btScalar((trimesh->vertices[trimesh->faces[i].id3][2]-com[2])*sc));
btTrimesh->addTriangle(v1,v2,v3);
}
diff --git a/VirtualRobot/Obstacle.cpp b/VirtualRobot/Obstacle.cpp
index 4a0a7d92c6e2d92242af10c3ac9a6d7c5d7dfa03..37ba1933cd8f505dffcbe0b3ace0cdd5c4ec7e48 100644
--- a/VirtualRobot/Obstacle.cpp
+++ b/VirtualRobot/Obstacle.cpp
@@ -58,7 +58,14 @@ VirtualRobot::ObstaclePtr Obstacle::createBox( float width, float height, float
VR_ERROR << "Could not create factory for visu type " << visualizationType << endl;
return result;
}
- VisualizationNodePtr visu = visualizationFactory->createBox(width,height,depth,color.r,color.g,color.b);
+ /*
+ std::vector<Primitive::PrimitivePtr> primitives;
+ Primitive::PrimitivePtr p(new Primitive::Box(width,height,depth));
+ primitives.push_back(p);
+
+ VisualizationNodePtr visu = visualizationFactory->getVisualizationFromPrimitives(primitives,false,color);
+ */
+ VisualizationNodePtr visu = visualizationFactory->createBox(width,height,depth,color.r,color.g,color.b);
if (!visu)
{
VR_ERROR << "Could not create box visualization with visu type " << visualizationType << endl;
@@ -97,6 +104,11 @@ VirtualRobot::ObstaclePtr Obstacle::createSphere( float radius, VisualizationFac
return result;
}
VisualizationNodePtr visu = visualizationFactory->createSphere(radius,color.r,color.g,color.b);
+ /*std::vector<Primitive::PrimitivePtr> primitives;
+ Primitive::PrimitivePtr p(new Primitive::Sphere(radius));
+ primitives.push_back(p);
+ VisualizationNodePtr visu = visualizationFactory->getVisualizationFromPrimitives(primitives,false,color);
+ */
if (!visu)
{
VR_ERROR << "Could not create sphere visualization with visu type " << visualizationType << endl;
diff --git a/VirtualRobot/Primitive.h b/VirtualRobot/Primitive.h
index 67d28e089e3f5f19f8e845fa8d0e6cfbdee16820..0dece7e35f8e6bd17ebb53e5e2cc3850ef1556f0 100644
--- a/VirtualRobot/Primitive.h
+++ b/VirtualRobot/Primitive.h
@@ -1,13 +1,14 @@
#ifndef PRIMITIVE_H
#define PRIMITIVE_H
+#include <VirtualRobot/VirtualRobotImportExport.h>
#include <Eigen/Geometry>
#include <boost/format.hpp>
namespace VirtualRobot {
namespace Primitive {
-class Primitive {
+class VIRTUAL_ROBOT_IMPORT_EXPORT Primitive {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
static const int TYPE = 0;
@@ -24,20 +25,22 @@ private:
Primitive() : type(TYPE) {}
};
-class Box : public Primitive {
+class VIRTUAL_ROBOT_IMPORT_EXPORT Box : public Primitive {
public:
static const int TYPE = 1;
Box() : Primitive(TYPE) {}
+ Box(float with, float height, float depth) : Primitive(TYPE), width(width), height(height), depth(depth) {}
float width;
float height;
float depth;
std::string toXMLString(int tabs = 0);
};
-class Sphere : public Primitive {
+class VIRTUAL_ROBOT_IMPORT_EXPORT Sphere : public Primitive {
public:
static const int TYPE = 2;
Sphere() : Primitive(TYPE) {}
+ Sphere(float radius) : Primitive(TYPE), radius(radius) {}
float radius;
std::string toXMLString(int tabs = 0);
};
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index 9e92885b1e1f225f71d16a122feb6ecc54a8d838..b59fb2dd7b7d490e55408ac295178c3cec7a23f0 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -78,7 +78,7 @@ namespace VirtualRobot {
* \param boundingBox Use bounding box instead of full model.
* \return instance of VirtualRobot::CoinVisualizationNode upon success and VirtualRobot::VisualizationNode on error.
*/
- VisualizationNodePtr CoinVisualizationFactory::getVisualizationFromPrimitives(const std::vector<Primitive::PrimitivePtr> &primitives, bool boundingBox)
+ VisualizationNodePtr CoinVisualizationFactory::getVisualizationFromPrimitives(const std::vector<Primitive::PrimitivePtr> &primitives, bool boundingBox, Color color)
{
VisualizationNodePtr visualizationNode = VisualizationNodePtr(new VisualizationNode());
SoSeparator *coinVisualization = new SoSeparator();
@@ -90,7 +90,7 @@ namespace VirtualRobot {
Primitive::PrimitivePtr p = *it;
currentTransform *= p->transform;
SoSeparator *soSep = new SoSeparator();
- SoNode *pNode = GetNodeFromPrimitive(p, boundingBox);
+ SoNode *pNode = GetNodeFromPrimitive(p, boundingBox, color);
soSep->addChild(getMatrixTransformScaleMM2M(currentTransform));
soSep->addChild(pNode);
coinVisualization->addChild(soSep);
@@ -113,9 +113,12 @@ namespace VirtualRobot {
return visualizationNode;
}
- SoNode* CoinVisualizationFactory::GetNodeFromPrimitive(Primitive::PrimitivePtr primitive, bool boundingBox)
+ SoNode* CoinVisualizationFactory::GetNodeFromPrimitive(Primitive::PrimitivePtr primitive, bool boundingBox, Color color)
{
- SoNode *coinVisualization;
+ SoSeparator *coinVisualization = new SoSeparator;
+ SoNode* c = getColorNode(color);
+ coinVisualization->addChild(c);
+
if (primitive->type == Primitive::Box::TYPE)
{
Primitive::Box *box = boost::dynamic_pointer_cast<Primitive::Box>(primitive).get();
@@ -123,21 +126,19 @@ namespace VirtualRobot {
soBox->width = box->width / 1000.f;
soBox->height = box->height / 1000.f;
soBox->depth = box->depth / 1000.f;
- coinVisualization = soBox;
+ coinVisualization->addChild(soBox);
} else if (primitive->type == Primitive::Sphere::TYPE)
{
Primitive::Sphere *sphere = boost::dynamic_pointer_cast<Primitive::Sphere>(primitive).get();
SoSphere *soSphere = new SoSphere;
soSphere->radius = sphere->radius / 1000.f;
- coinVisualization = soSphere;
+ coinVisualization->addChild(soSphere);
}
if (boundingBox && coinVisualization)
{
SoSeparator* bboxVisu = CreateBoundingBox(coinVisualization, false);
- bboxVisu->ref();
- coinVisualization->unref();
- coinVisualization = bboxVisu;
+ coinVisualization->addChild(bboxVisu);
}
return coinVisualization;
@@ -3017,6 +3018,14 @@ SoGroup* CoinVisualizationFactory::convertSoFileChildren(SoGroup* orig)
return storeResult;
}
+SoNode* CoinVisualizationFactory::getColorNode( Color color )
+{
+ SoMaterial *m = new SoMaterial();
+ m->ambientColor.setValue(color.r,color.g,color.b);
+ m->diffuseColor.setValue(color.r,color.g,color.b);
+ return m;
+}
+
} // namespace VirtualRobot
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
index d76f3216e31d458b72c543e689a256f6290a4960..2b8c383fc094de5e746246cb71e66bc190ba80c0 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
@@ -59,7 +59,7 @@ public:
CoinVisualizationFactory();
virtual ~CoinVisualizationFactory();
- virtual VisualizationNodePtr getVisualizationFromPrimitives(const std::vector<Primitive::PrimitivePtr> &primitives, bool boundingBox = false);
+ virtual VisualizationNodePtr getVisualizationFromPrimitives(const std::vector<Primitive::PrimitivePtr> &primitives, bool boundingBox = false, Color color = Color::Gray());
virtual VisualizationNodePtr getVisualizationFromFile(const std::string& filename, bool boundingBox = false);
virtual VisualizationNodePtr getVisualizationFromSTLFile(const std::string& filename, bool boundingBox = false);
virtual VisualizationNodePtr getVisualizationFromCoin3DFile(const std::string& filename, bool boundingBox = false);
@@ -174,6 +174,10 @@ public:
*/
static SoSeparator* CreateEndEffectorVisualization(EndEffectorPtr eef, SceneObject::VisualizationType = SceneObject::Full);
+ /*!
+ Creates a material node.
+ */
+ static SoNode* getColorNode(Color color);
/*!
Text visu. Offsets in mm.
*/
@@ -302,7 +306,7 @@ public:
*/
virtual void cleanup();
protected:
- static SoNode* GetNodeFromPrimitive(Primitive::PrimitivePtr primitive, bool boundingBox);
+ static SoNode* GetNodeFromPrimitive(Primitive::PrimitivePtr primitive, bool boundingBox, Color color);
static void GetVisualizationFromSoInput(SoInput& soInput, VisualizationNodePtr& visualizationNode, bool bbox = false);
static inline char IVToolsHelper_ReplaceSpaceWithUnderscore(char input) { if ( ' ' == input ) return '_'; else return input; }
diff --git a/VirtualRobot/Visualization/VisualizationFactory.h b/VirtualRobot/Visualization/VisualizationFactory.h
index cc27002b6d3db03b8bf071c5291482c05df05d6f..226a91cfeb3c38c2c9fd5848954f425d9a2f1d70 100644
--- a/VirtualRobot/Visualization/VisualizationFactory.h
+++ b/VirtualRobot/Visualization/VisualizationFactory.h
@@ -74,7 +74,7 @@ public:
VisualizationFactory() {;}
virtual ~VisualizationFactory() {;}
- virtual VisualizationNodePtr getVisualizationFromPrimitives(const std::vector<Primitive::PrimitivePtr> &primitives, bool boundingBox = false){return VisualizationNodePtr();}
+ virtual VisualizationNodePtr getVisualizationFromPrimitives(const std::vector<Primitive::PrimitivePtr> &primitives, bool boundingBox = false, Color color = Color::Gray()){return VisualizationNodePtr();}
virtual VisualizationNodePtr getVisualizationFromFile(const std::string& filename, bool boundingBox = false){return VisualizationNodePtr();}
virtual VisualizationNodePtr getVisualizationFromFile(const std::ifstream& ifs, bool boundingBox = false){return VisualizationNodePtr();}
/*!
diff --git a/VirtualRobot/Workspace/WorkspaceDataArray.cpp b/VirtualRobot/Workspace/WorkspaceDataArray.cpp
index 36b8a69760a2175df79d913f393d21c8f70d6315..45766a4f242db5240b51c0acbb8842d1517ea400 100644
--- a/VirtualRobot/Workspace/WorkspaceDataArray.cpp
+++ b/VirtualRobot/Workspace/WorkspaceDataArray.cpp
@@ -416,9 +416,9 @@ bool WorkspaceDataArray::save(std::ofstream &file)
CompressionBZip2Ptr bzip2(new CompressionBZip2(&file));
unsigned char* emptyData = new unsigned char[getSizeRot()];
memset(emptyData, 0, getSizeRot() * sizeof(unsigned char));
- for (int x = 0; x < sizes[0]; x++)
- for (int y = 0; y < sizes[1]; y++)
- for (int z = 0; z < sizes[2]; z++)
+ for (unsigned int x = 0; x < sizes[0]; x++)
+ for (unsigned int y = 0; y < sizes[1]; y++)
+ for (unsigned int z = 0; z < sizes[2]; z++)
{
void* dataBlock;
// this avoids that an empty data block is created within the workspace data when no data is available.