#include <iostream.h>
#include <Inventor/Xt/SoXt.h>
#include <Inventor/Xt/viewers/SoXtExaminerViewer.h>
#include <Inventor/nodes/SoDirectionalLight.h>
#include <Inventor/nodes/SoNormal.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoPerspectiveCamera.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoTranslation.h>
#include <Inventor/nodes/SoRotation.h>
#include <Inventor/nodes/SoPointLight.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoIndexedFaceSet.h>
#include <Inventor/nodes/SoSwitch.h>
#include <Inventor/nodes/SoCone.h>
#include <Inventor/nodes/SoCylinder.h>
#include <Inventor/nodes/SoSphere.h>
#include <Inventor/SoInput.h>
#include <Inventor/SoDB.h>
#include <Inventor/sensors/SoIdleSensor.h>
#include <Inventor/sensors/SoSensor.h>
#include <Inventor/nodes/SoEventCallback.h>
#include <Inventor/events/SoKeyboardEvent.h>
#include "model.h"
#include "rom.h"
#include "collisionset.h"
#include "romanglepool.h"
Include dependency graph for rom.c++:
Defines | |
#define | DPHI 1.0 |
#define | THETA_RESOLUTION .1 |
Functions | |
void | idle (void *data, SoSensor *s) |
callback function for Idle-Sensor | |
void | keyCB (void *userData, SoEventCallback *eventCB) |
callback function for keyboard event | |
int | main (int, char **argv) |
Main: this is where we start. | |
Variables | |
SoSeparator * | root |
root node of Inventor tree | |
SoSeparator * | movables |
root node of Inventor movable nodes | |
CollisionSet * | fixed |
collsion set that contains the model of all fixed elements (non-moving) | |
CollisionSet * | movable |
coolision set that contains the models of all movable elements | |
SoTranslation * | movableTranslation |
Inventor translation for movable objects. | |
SoRotation * | movableRotation |
Inventor Rotation for movable objects. | |
RomAnglePool * | romAnglePool |
data structore for holding the rom data | |
bool | firstPass = true |
true if romputation of rom data is not completed yet | |
bool | visualisation = false |
booelan flag: true if ROM cone is visible | |
SoSwitch * | visualisationSwitch |
switch node to enable/disable visualisation of ROM cone | |
bool | axisVisible = true |
boolean flag: true if axis are visiable | |
SoSwitch * | axisSwitch |
switch node to enable/disable axis | |
double | phi = 0.0 |
phi = orientation of rotation axis | |
double | theta = 0.0 |
theta = elongation of leg | |
bool | running = false |
true if animation is running |
|
|
|
|
|
callback function for Idle-Sensor This callback function is called whenever Openinventor is in idle state. During idle state the next simulation step is computed.
00301 { 00302 double step = 90; 00303 theta = 0; 00304 SbVec3f axis; 00305 00306 while (step > THETA_RESOLUTION/2.0) { 00307 step = step/2.0; 00308 00309 // compute new elongation 00310 double ux = 0; // normalized rotation axis (u) 00311 double uy = -sin(phi*M_PI/180.0); 00312 double uz = -cos(phi*M_PI/180.0); 00313 axis.setValue(ux, uy, uz); 00314 00315 // trace positioning 00316 // movableRotation->rotation.setValue(axis, theta*M_PI/180.0); 00317 00318 // compute rotation matrix of rotation using quaterions 00319 // Q = ( q0, (q1, q2, q3) ) 00320 // u = axis vector (normalized) 00321 // theta = rotation angle 00322 double q0 = cos(theta*M_PI/360.0); // q0 = cos(theta/2.0) 00323 double s = sin(theta*M_PI/360.0); 00324 double q1 = ux*s; // q1 = ux * sin(theta/2) 00325 double q2 = uy*s; // q2 = uy * sin(theta/2) 00326 double q3 = uz*s; // q3 = uz * sin(theta/2) 00327 00328 // translate quaterion back into matrix 00329 double R[3][3]; 00330 R[0][0] = 1 - 2*q2*q2 - 2*q3*q3; 00331 R[0][1] = 2*q1*q2 - 2*q0*q3; 00332 R[0][2] = 2*q1*q3 + 2*q0*q2 ; 00333 R[1][0] = 2*q1*q2 + 2*q0*q3; 00334 R[1][1] = 1 - 2*q1*q1 - 2*q3*q3; 00335 R[1][2] = 2*q2*q3 - 2*q0*q1; 00336 R[2][0] = 2*q1*q3 - 2*q0*q2; 00337 R[2][1] = 2*q2*q3 + 2*q0*q1; 00338 R[2][2] = 1 - 2*q1*q1 - 2*q2*q2; 00339 movable->setOrientation(R); 00340 00341 if (CollisionSet::checkForCollision(fixed, movable)) { 00342 theta += -step; // collision 00343 } else { 00344 theta += step; // no collision 00345 } 00346 } 00347 00348 // only display end position 00349 movableRotation->rotation.setValue(axis, theta*M_PI/180.0); 00350 if (firstPass) { 00351 // add rom data into rom angle pool 00352 romAnglePool->append(phi,theta); 00353 } 00354 00355 // next phi 00356 phi = phi+DPHI; 00357 if (phi >= 360) { 00358 phi = phi-360; 00359 00360 if (firstPass) { 00361 firstPass = false; 00362 visualisationSwitch->addChild(romAnglePool->createVisualizationCone()); 00363 if (visualisation) { 00364 visualisationSwitch->whichChild = 1; 00365 } 00366 } 00367 } 00368 if (running) { 00369 sensor->schedule(); 00370 } 00371 } |
|
callback function for keyboard event This callback is called whenever the user hits a key. It toggles the cone visualisation when 'C' is pressed.
00381 { 00382 SoIdleSensor* idleSensor = (SoIdleSensor*)(userData); 00383 if (SO_KEY_PRESS_EVENT(eventCB->getEvent(), SoKeyboardEvent::C)) { 00384 visualisation = !visualisation; 00385 if (visualisation) { 00386 visualisationSwitch->whichChild = 1; 00387 } else { 00388 visualisationSwitch->whichChild = 0; 00389 } 00390 } else if (SO_KEY_PRESS_EVENT(eventCB->getEvent(), SoKeyboardEvent::A)) { 00391 axisVisible = !axisVisible; 00392 if (axisVisible) { 00393 axisSwitch->whichChild = 1; 00394 } else { 00395 axisSwitch->whichChild = 0; 00396 } 00397 } else if (SO_KEY_PRESS_EVENT(eventCB->getEvent(), SoKeyboardEvent::S)) { 00398 running = !running; 00399 if (running) { 00400 idleSensor->schedule(); 00401 } 00402 } 00403 } |
|
Main: this is where we start. setup cone material
00092 { 00093 Widget myWindow = SoXt::init(argv[0]); 00094 if (myWindow == NULL) exit(1); 00095 00096 root = new SoSeparator; 00097 root->ref(); 00098 00099 // add key event listener 00100 SoIdleSensor *idleSensor = new SoIdleSensor; 00101 SoEventCallback *eventCB = new SoEventCallback; 00102 eventCB->addEventCallback(SoKeyboardEvent::getClassTypeId(), keyCB, idleSensor); 00103 root->addChild(eventCB); 00104 00105 // select light source 00106 SoPointLight *light = new SoPointLight; 00107 light->location.setValue(100.0, 100.0, 100.0); 00108 light->intensity = 0.5; 00109 light->on = true; 00110 root->addChild(light); 00111 00112 Color diffuse, specular; 00113 00114 axisSwitch = new SoSwitch; 00115 root->addChild(axisSwitch); 00116 axisSwitch->addChild(new SoSeparator); // child 0 = nothing (invisible) 00117 SoSeparator* axisNode = new SoSeparator; 00118 axisSwitch->addChild(axisNode); 00119 00120 // draw xaxis 00121 SoCone *xcone = new SoCone; 00122 SoCylinder *xcyl = new SoCylinder; 00123 xcone->bottomRadius = 3.0; 00124 xcone->height = 12; 00125 xcyl->radius = 1.0; 00126 xcyl->height = 200; 00127 SoRotation *xrt = new SoRotation; 00128 SbVec3f axis; 00129 axis.setValue(0.0, 0.0, 1.0); 00130 xrt->rotation.setValue(axis, -M_PI/2.0); 00131 SoTranslation *xtr1 = new SoTranslation; 00132 xtr1->translation.setValue(0,100,0); 00133 SoTranslation *xtr2 = new SoTranslation; 00134 xtr2->translation.setValue(0, 106, 0); 00135 SoMaterial *xmat = new SoMaterial; 00136 xmat->diffuseColor.setValue(1.0, 0.0, 0.0); 00137 xmat->specularColor.setValue(1.0, 1.0, 1.0); 00138 xmat->ambientColor.setValue(0.0, 0.0, 0.0); 00139 SoSeparator *xaxis = new SoSeparator; 00140 axisNode->addChild(xaxis); 00141 xaxis->addChild(xmat); 00142 xaxis->addChild(xrt); 00143 xaxis->addChild(xtr1); 00144 xaxis->addChild(xcyl); 00145 xaxis->addChild(xtr2); 00146 xaxis->addChild(xcone); 00147 00148 // draw yaxis 00149 SoCone *ycone = new SoCone; 00150 SoCylinder *ycyl = new SoCylinder; 00151 ycone->bottomRadius = 3.0; 00152 ycone->height = 12; 00153 ycyl->radius = 1.0; 00154 ycyl->height = 200; 00155 SoTranslation *ytr1 = new SoTranslation; 00156 ytr1->translation.setValue(0,100,0); 00157 SoTranslation *ytr2 = new SoTranslation; 00158 ytr2->translation.setValue(0, 106, 0); 00159 SoMaterial *ymat = new SoMaterial; 00160 ymat->diffuseColor.setValue(0.0, 1.0, 0.0); 00161 ymat->specularColor.setValue(1.0, 1.0, 1.0); 00162 ymat->ambientColor.setValue(0.0, 0.0, 0.0); 00163 SoSeparator *yaxis = new SoSeparator; 00164 axisNode->addChild(yaxis); 00165 yaxis->addChild(ymat); 00166 yaxis->addChild(ytr1); 00167 yaxis->addChild(ycyl); 00168 yaxis->addChild(ytr2); 00169 yaxis->addChild(ycone); 00170 00171 // draw zaxis 00172 SoCone *zcone = new SoCone; 00173 SoCylinder *zcyl = new SoCylinder; 00174 zcone->bottomRadius = 3.0; 00175 zcone->height = 12; 00176 zcyl->radius = 1.0; 00177 zcyl->height = 200; 00178 SoRotation *zrt = new SoRotation; 00179 axis.setValue(1.0, 0.0, 0.0); 00180 zrt->rotation.setValue(axis, M_PI/2.0); 00181 SoTranslation *ztr1 = new SoTranslation; 00182 ztr1->translation.setValue(0, 100, 0); 00183 SoTranslation *ztr2 = new SoTranslation; 00184 ztr2->translation.setValue(0, 106, 0); 00185 SoMaterial *zmat = new SoMaterial; 00186 zmat->diffuseColor.setValue(0.0, 0.0, 1.0); 00187 zmat->specularColor.setValue(1.0, 1.0, 1.0); 00188 zmat->ambientColor.setValue(0.0, 0.0, 0.0); 00189 SoSeparator *zaxis = new SoSeparator; 00190 axisNode->addChild(zaxis); 00191 zaxis->addChild(zmat); 00192 zaxis->addChild(zrt); 00193 zaxis->addChild(ztr1); 00194 zaxis->addChild(zcyl); 00195 zaxis->addChild(ztr2); 00196 zaxis->addChild(zcone); 00197 00198 // axis is initially visible 00199 axisSwitch->whichChild = 1; 00200 00201 /* open fixed objects */ 00202 // read antilux 00203 Model *antilux = new Model(ANTILUX_MODEL); 00204 antilux->getMaterial()->diffuseColor.setValue(0.1, 0.5, 0.0); 00205 antilux->getMaterial()->specularColor.setValue(0.2, 0.8, 0.0); 00206 antilux->getMaterial()->shininess = 0.5; 00207 root->addChild(antilux->getRootModel()); 00208 00209 // read bicon 00210 Model *bicon = new Model(BICON_MODEL); 00211 bicon->getMaterial()->diffuseColor.setValue(0.5, 0.1, 0.0); 00212 bicon->getMaterial()->specularColor.setValue(0.8, 0.2, 0.0); 00213 bicon->getMaterial()->shininess = 0.5; 00214 root->addChild(bicon->getRootModel()); 00215 00216 // read hip 00217 Model *hip = new Model(HIP_MODEL); 00218 hip->getMaterial()->diffuseColor.setValue(0.6, 0.5, 0.3); 00219 hip->getMaterial()->specularColor.setValue(0.0, 0.0, 0.0); 00220 hip->getMaterial()->shininess = 0.5; 00221 root->addChild(hip->getRootModel()); 00222 00223 /* open movable objects */ 00224 movables = new SoSeparator; 00225 movableTranslation = new SoTranslation; 00226 movableRotation = new SoRotation; 00227 movables->addChild(movableTranslation); 00228 movables->addChild(movableRotation); 00229 root->addChild(movables); 00230 00231 // read shaft 00232 Model *shaft = new Model(SHAFT_MODEL); 00233 shaft->getMaterial()->diffuseColor.setValue(0.5, 0.5, 0.5); 00234 shaft->getMaterial()->specularColor.setValue(0.7, 0.7, 0.7); 00235 shaft->getMaterial()->shininess = 0.5; 00236 movables->addChild(shaft->getRootModel()); 00237 00238 // read femur 00239 Model *femur = new Model(FEMUR_MODEL); 00240 femur->getMaterial()->diffuseColor.setValue(0.6, 0.5, 0.3); 00241 femur->getMaterial()->specularColor.setValue(0.0, 0.0, 0.0); 00242 femur->getMaterial()->shininess = 0.99; 00243 movables->addChild(femur->getRootModel()); 00244 00245 // set up collision sets 00246 fixed = new CollisionSet; 00247 fixed->insert(hip); 00248 fixed->insert(antilux); 00249 fixed->insert(bicon); 00250 00251 movable = new CollisionSet; 00252 movable->insert(femur); 00253 movable->insert(shaft); 00254 00255 romAnglePool = new RomAnglePool; 00256 00257 00259 SoMaterial *conemat = new SoMaterial; 00260 conemat->diffuseColor.setValue(1.0, 0.0, 0.0); 00261 conemat->specularColor.setValue(1.0, 1.0, 1.0); 00262 conemat->ambientColor.setValue(0.0, 0.0, 0.0); 00263 root->addChild(conemat); 00264 visualisationSwitch = new SoSwitch; 00265 root->addChild(visualisationSwitch); 00266 visualisationSwitch->addChild(new SoSeparator); // child 0 is dummy 00267 visualisationSwitch->whichChild = 0; 00268 00269 00270 // setup idle sensor 00271 idleSensor->setFunction(idle); 00272 idleSensor->setData(root); 00273 if (running) { 00274 idleSensor->schedule(); 00275 } 00276 00277 // Set up viewer: 00278 SoXtExaminerViewer *myViewer = 00279 new SoXtExaminerViewer(myWindow); 00280 myViewer->setSceneGraph(root); 00281 myViewer->setTitle("Examiner Viewer"); 00282 myViewer->setBackgroundColor(SbColor(0.0, 0.0, 0.3)); 00283 //myViewer->setViewing(false); 00284 00285 myViewer->show(); 00286 00287 SoXt::show(myWindow); 00288 SoXt::mainLoop(); 00289 } |
|
switch node to enable/disable axis
|
|
boolean flag: true if axis are visiable
|
|
true if romputation of rom data is not completed yet
|
|
collsion set that contains the model of all fixed elements (non-moving)
|
|
coolision set that contains the models of all movable elements
|
|
Inventor Rotation for movable objects.
|
|
root node of Inventor movable nodes
|
|
Inventor translation for movable objects.
|
|
phi = orientation of rotation axis
|
|
data structore for holding the rom data
|
|
root node of Inventor tree
|
|
true if animation is running
|
|
theta = elongation of leg
|
|
booelan flag: true if ROM cone is visible
|
|
switch node to enable/disable visualisation of ROM cone
|