//===========================================================================
cCamera::cCamera(cWorld* a_parentWorld)
{
// set default values for clipping planes
setClippingPlanes(0.1, 1000.0);
// set default field of view angle
setFieldViewAngle(45);
// set parent world
m_parentWorld = a_parentWorld;
// position and orient camera, looking down the negative x-axis
// (the robotics convention)
set(
cVector3d(0,0,0), // Local Position of camera.
cVector3d(-1,0,0), // Local Look At position
cVector3d(0,0,1) // Local Up Vector
);
// set default stereo parameters
m_stereoFocalLength = 5.0;
m_stereoEyeSeparation = 0.5;
// disable multipass transparency rendering by default
m_useMultipassTransparency = 0;
m_performingDisplayReset = 0;
memset(m_projectionMatrix,0,sizeof(m_projectionMatrix));
}
void Carro::rotacionarEixos(double delta){
if(fabs(steeringAngle+delta)<=ANGULOLIMITE) {
steeringAngle += delta;
eixo1->rotate(cVector3d(0,1,0),delta);
eixo2->rotate(cVector3d(0,1,0),delta);
}
}
cMatrix3d InputManager::GetCameraTransformations()
{
cMatrix3d cam;
//Col0 has Position
cam.setCol0(cVector3d(transforms.xPos, transforms.yPos, transforms.zPos));
//Col1 has Gaze
dvec4 gazeCamSpace = dvec4( 0, 0, 1, 1);
dvec4 gazeWorldSpace = camToWorld * gazeCamSpace;
cam.setCol1(cVector3d(gazeWorldSpace.x, gazeWorldSpace.y, gazeWorldSpace.z));
//Col2 has Up
cVector3d up;
if(transforms.elevation == 0)
{
//If Looking straight down, change the up vector to the heading
up = cVector3d(cos(d2r(transforms.heading)), 0, sin(d2r(transforms.heading)));
}
else if(transforms.elevation == 180)
{
//If looking straight up, change the up vector to the negative heading
up = cVector3d(0 - cos(d2r(transforms.heading)), 0, 0 - sin(d2r(transforms.heading)));
}
else
{
up = cVector3d(0,1,0);
}
cam.setCol2(up);
return cam;
}
void keySelect(unsigned char key, int x, int y)
{
// escape key
if ((key == 27) || (key == 'x'))
{
// close everything
close();
// exit application
exit(0);
}
// option 1:
if (key == '1')
{
// enable gravity
ODEWorld->setGravity(cVector3d(0.0, 0.0, -9.81));
}
// option 2:
if (key == '2')
{
// disable gravity
ODEWorld->setGravity(cVector3d(0.0, 0.0, 0.0));
}
}
//==============================================================================
void cDrawArrow(const cVector3d& a_arrowStart,
const cVector3d& a_arrowTip,
const double a_width)
{
#ifdef C_USE_OPENGL
glPushMatrix();
// We don't really care about the up vector, but it can't
// be parallel to the arrow...
cVector3d up = cVector3d(0,1,0);
cVector3d arrow = a_arrowTip-a_arrowStart;
arrow.normalize();
double d = fabs(cDot(up,arrow));
if (d > .9)
{
up = cVector3d(1,0,0);
}
cLookAt(a_arrowStart, a_arrowTip, up);
double distance = cDistance(a_arrowTip,a_arrowStart);
// This flips the z axis around
glRotatef(180,1,0,0);
// create a new OpenGL quadratic object
GLUquadricObj *quadObj;
quadObj = gluNewQuadric();
#define ARROW_CYLINDER_PORTION 0.75
#define ARRROW_CONE_PORTION (1.0 - 0.75)
// set rendering style
gluQuadricDrawStyle(quadObj, GLU_FILL);
// set normal-rendering mode
gluQuadricNormals(quadObj, GLU_SMOOTH);
// render a cylinder and a cone
glRotatef(180,1,0,0);
gluDisk(quadObj,0,a_width,10,10);
glRotatef(180,1,0,0);
gluCylinder(quadObj, a_width, a_width, distance*ARROW_CYLINDER_PORTION, 10, 10);
glTranslated(0, 0, ARROW_CYLINDER_PORTION*distance);
glRotatef(180, 1, 0, 0);
gluDisk(quadObj, 0, a_width*2.0, 10, 10);
glRotatef(180,1,0,0);
gluCylinder(quadObj, a_width*2.0, 0.0, distance*ARRROW_CONE_PORTION, 10, 10);
// delete our quadric object
gluDeleteQuadric(quadObj);
glPopMatrix();
#endif
}
void HugMe::InitRemoteWorld()
{
m_worldRemote = new cWorld();
// Create a mesh - we will build a cube manually, and later let the
// user load 3d models
m_remoteHumanModel.SetParentWorld(m_worldRemote);
m_remoteHumanModel.ShowAvatar(m_config.m_bShowAvatar);
// for debug
m_pContactPoint = new cMesh(m_worldRemote);
m_worldRemote->addChild(m_pContactPoint);
m_pContactPoint->loadFromFile("Blender/ContactPoint.obj");
m_pContactPoint->translate(CHAI_LARGE, CHAI_LARGE, CHAI_LARGE);
m_pContactPoint->computeGlobalPositions();
m_pContactPoint->setHapticEnabled(false, true);
ShowContactPoint(m_config.m_bShowContactpoint);
// set camera position and orientation
//
// We choose to put it out on the positive z axis, so things appear
// the way OpenGL users expect them to appear, with z going in and
// out of the plane of the screen.
double angle;
angle = atan(240.0*m_pixelWidth/m_focalLength)*180.0/3.141592;
if(!m_camera) {
m_camera = new cCamera(m_worldRemote);
int result = m_camera->set(cVector3d(0.0,0.0,0.0), // position of camera
cVector3d(0.0, 0.0, -1.0), // camera looking at origin
cVector3d(0.0, 1.0, 0.0)); // orientation of camera (standing up)
m_camera->setFieldViewAngle(angle);
m_camera->setClippingPlanes(1.0, (double)m_primaryDistance+(double)m_depthDistance);
}
if(!m_light) {
// Turn on one light...
m_light = new cLight(m_worldRemote);
m_light->setEnabled(true);
// Put the light somewhere that looks nice
m_light->setDirectionalLight(true);
m_light->setPos(cVector3d(1000.0, 1000.0, 1000));
m_light->setDir(cVector3d(0.0, 0.0, -1.0));
m_light->m_ambient.set(0.6f, 0.6f, 0.6f, 1.0f);
m_light->m_diffuse.set(0.7f, 0.7f, 0.7f, 1.0f);
m_light->m_specular.set(0.5f, 0.5f, 0.5f, 1.0f);
// Don't make it a spotlight
m_light->setCutOffAngle(120.0);
}
// Create a display for graphic rendering
m_viewport = new cViewport(m_hWnd, m_camera, false);
}
void Carro::movimentar(double passo){
roda1->rotate(cVector3d(0,0,1),passo);
roda2->rotate(cVector3d(0,0,1),passo);
roda3->rotate(cVector3d(0,0,1),passo);
roda4->rotate(cVector3d(0,0,1),passo);
double dangle = -passo*RAIORODA*tan(steeringAngle)/DISTANCIAEIXOS;
carAngle+= dangle;
meio->rotate(cVector3d(0,1,0),dangle);
this->translate(-passo*RAIORODA*cos(carAngle),0,passo*RAIORODA*sin(carAngle));
}
//===========================================================================
bool cEffectVibration::computeForce(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int& a_toolID,
cVector3d& a_reactionForce)
{
if (m_parent->m_interactionInside)
{
// read vibration parameters
double vibrationFrequency = m_parent->m_material.getVibrationFrequency();
double vibrationAmplitude = m_parent->m_material.getVibrationAmplitude();
// read time
double time = clock.getCurrentTimeSeconds();
// compute force magnitude
double forceMag = vibrationAmplitude * sin(2.0 * CHAI_PI *vibrationFrequency * time);
a_reactionForce = cMul(forceMag, cVector3d(1, 0, 0));
return (true);
}
else
{
// the tool is located outside the object, so zero reaction force
a_reactionForce.zero();
return (false);
}
}
cVector3d InputManager::RotateVector(const cVector3d& vec)
{
dvec4 glmVec = dvec4(vec.x, vec.y, vec.z, 1);
dvec4 newVec = rotateToWorld * glmVec;
//might need to negate z coordinate
return cVector3d(newVec.x, newVec.y, 0 - newVec.z);
}
void updateCameraPosition()
{
// check values
if (cameraDistance < 0.1) { cameraDistance = 0.1; }
if (cameraAngleV > 89) { cameraAngleV = 89; }
if (cameraAngleV < 10) { cameraAngleV = 10; }
// compute position of camera in space
cVector3d pos = cAdd(
cameraPosition,
cVector3d(
cameraDistance * cCosDeg(cameraAngleH) * cCosDeg(cameraAngleV),
cameraDistance * cSinDeg(cameraAngleH) * cCosDeg(cameraAngleV),
cameraDistance * cSinDeg(cameraAngleV)
)
);
// compute lookat position
cVector3d lookat = cameraPosition;
// define role orientation of camera
cVector3d up(0.0, 0.0, 1.0);
// set new position to camera
camera->set(pos, lookat, up);
// recompute global positions
world->computeGlobalPositions(true);
}
// overriden from class cProxyPointForceAlgo
cVector3d ch_proxyPointForceAlgo::computeForcesD(const cVector3d& a_toolPos, const cVector3d& a_toolVel)
{
// update device position
m_deviceGlobalPos = a_toolPos;
// check if world has been defined; if so, compute forces
if (m_world != NULL)
{
// compute next best position of proxy
computeNextBestProxyPosition(m_deviceGlobalPos, a_toolVel);
// update proxy to next best position
m_proxyGlobalPos = m_nextBestProxyGlobalPos;
// compute force vector applied to device
updateForce();
// return result
return (m_lastGlobalForce);
}
// if no world has been defined in which algorithm operates, there is no force
else
{
return (cVector3d(0.0, 0.0, 0.0));
}
}
void HapticLoop(void* param)
{
// simulation in now ON
Crecord_playerApp* app = (Crecord_playerApp*)(param);
// read position from haptic device
app->tool->updatePose();
// compute forces
app->tool->computeForces();
// get last interaction force in global coordinate frame
app->m_interactionForce = cMul(cTrans(app->object->getRot()), cSub(app->tool->m_lastComputedGlobalForce, app->object->getPos()));
app->tool->applyForces();
// figure out if we're touching the record
cProxyPointForceAlgo * algo = app->tool->getProxy();
if (algo->getContactObject() == app->m_recordMesh)
{
if (!app->m_inContact)
{
app->m_inContact = true;
app->m_RFDInitialAngle = app->m_rotPos - app->m_lastGoodPosition*CHAI_PI/180;
}
app->animateObject(app->m_interactionForce);
}
else
{
app->animateObject(cVector3d(0.0, 0.0, 0.0));
app->m_inContact = false;
}
}
//===========================================================================
cCamera::cCamera(cWorld* a_parentWorld)
{
// set parent world
m_parentWorld = a_parentWorld;
// set default values for clipping planes
setClippingPlanes(0.1, 1000.0);
// set default field of view angle
setFieldViewAngle(45);
// position and orient camera, looking down the negative x-axis
// (the robotics convention)
set(
cVector3d(0,0,0), // Local Position of camera.
cVector3d(-1,0,0), // Local Look At position
cVector3d(0,0,1) // Local Up Vector
);
// by default we use a persepctive camera
m_perspectiveMode = true;
// width of orthographic view. (not active by default)
m_orthographicWidth = 0.0;
// set default stereo parameters
m_stereoFocalLength = 2.0;
m_stereoEyeSeparation = 0.07;
// disable multipass transparency rendering by default
m_useMultipassTransparency = false;
// enable shadow rendering
m_useShadowCasting = false;
// enable stereo display
m_useStereo = false;
// reset display status
m_resetDisplay = false;
// create front and back layers
m_frontLayer = new cWorld();
m_backLayer = new cWorld();
}
__fastcall TActiveFormX::TActiveFormX(TComponent* AOwner) : TActiveForm(AOwner)
{
// create a new world
world = new cWorld();
// set background color
world->setBackgroundColor(0.0f,0.0f,0.0f);
// Create a camera
camera = new cCamera(world);
world->addChild(camera);
// Create a light source and attach it to camera
light = new cLight(world);
light->setEnabled(true);
light->setPos(cVector3d(2,1,1));
// define camera position
cameraAngleH = 10;
cameraAngleV = 20;
cameraDistance = 2.0;
flagCameraInMotion = false;
updateCameraPosition();
camera->setClippingPlanes(0.01, 10.0);
// create a display for graphic rendering
viewport = new cViewport(Panel1->Handle, camera, true);
viewport->setStereoOn(false);
// create a mesh - we will build a simple cube, and later let the
// user load 3d models
object = new cMesh(world);
world->addChild(object);
// create a nice little cube
createCube(object, 0.2);
// define a material property for this object
cMaterial material;
material.m_ambient.set( 0.4, 0.2, 0.2, 1.0 );
material.m_diffuse.set( 0.8, 0.6, 0.6, 1.0 );
material.m_specular.set( 0.9, 0.9, 0.9, 1.0 );
material.setShininess(100);
material.setStiffness(20 );
object->m_material = material;
// update camera position
updateCameraPosition();
// don't start the haptics loop yet
flagSimulationOn = false;
flagHasExitedSimulation = true;
}
//===========================================================================
int cPhantomDevice::getPosition(cVector3d& a_position)
{
// check if drivers are installed
if (!m_driverInstalled) return (-1);
double x,y,z;
int error = hdPhantomGetPosition(m_deviceID, &x, &y, &z);
a_position = m_specifications.m_positionOffset + cVector3d(x, y, z);
estimateLinearVelocity(a_position);
return (error);
}
void OscPrismCHAI::on_size()
{
// reposition vertices
int i,n;
n = m_pPrism->getNumVertices();
for (i=0; i<n; i++) {
cVector3d pos = m_pPrism->getVertex(i)->getPos();
pos.elementMul(cVector3d(1.0/fabs(pos.x), 1.0/fabs(pos.y), 1.0/fabs(pos.z)));
pos.elementMul(m_size/2.0);
m_pPrism->getVertex(i)->setPos(pos);
}
}
int ODEObject::push_handler(const char *path, const char *types,
lo_arg **argv, int argc,
void *data, void *user_data)
{
OscObject *me = static_cast<OscObject*>(user_data);
ODEObject *ode_object = static_cast<ODEObject*>(me->special());
cVector3d(argv[0]->f, argv[1]->f, argv[2]->f).copyto(me->m_force);
dBodyAddForceAtPos(ode_object->body(),
argv[0]->f, argv[1]->f, argv[2]->f,
argv[3]->f, argv[4]->f, argv[5]->f);
return 0;
}
void updateGraphics(void)
{
// render world
camera->renderView(displayW, displayH);
// Swap buffers
glutSwapBuffers();
// check for any OpenGL errors
GLenum err;
err = glGetError();
if (err != GL_NO_ERROR) printf("Error: %s\n", gluErrorString(err));
// inform the GLUT window to call updateGraphics again (next frame)
if (simulationRunning)
{
glutPostRedisplay();
}
// rotate the following objcts to create some animation
object1->rotate(cVector3d(0,0,1), 0.02);
object3->rotate(cVector3d(1,1,1), -0.01);
}
OscCursorCHAI::OscCursorCHAI(cWorld *world, const char *name, OscBase *parent)
: OscSphere(NULL, name, parent)
{
// create the cursor object
m_pCursor = new cMeta3dofPointer(world);
world->addChild(m_pCursor);
// User data points to the OscObject, used for identification
// during object contact.
m_pCursor->setUserData(this, 1);
// replace the potential proxy algorithm with our own
cGenericPointForceAlgo *old_proxy, *new_proxy;
old_proxy = m_pCursor->m_pointForceAlgos[1];
new_proxy = new cODEPotentialProxy(
dynamic_cast<cPotentialFieldForceAlgo*>(old_proxy));
m_pCursor->m_pointForceAlgos[1] = new_proxy;
delete old_proxy;
if (m_pCursor->initialize()) {
m_bInitialized = false;
printf("[%s] Could not initialize.\n", simulation()->type_str());
} else {
m_bInitialized = true;
m_pCursor->start();
printf("[%s] Using %s device.\n",
simulation()->type_str(), device_str());
}
// rotate the cursor to match visual rotation
m_pCursor->rotate(cVector3d(0,0,1),-90.0*M_PI/180.0);
// make it a cursor tuned for a dynamic environment
((cProxyPointForceAlgo*)m_pCursor->m_pointForceAlgos[0])
->enableDynamicProxy(true);
// this is necessary for the above rotation to take effect
m_pCursor->computeGlobalPositions();
// set up mass as zero to begin (transparent proxy)
m_mass.set(0);
// no extra force to begin with
m_nExtraForceSteps = 0;
m_pSpecial = new CHAIObject(this, m_pCursor, world);
}
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
printf ("\n");
printf ("-----------------------------------\n");
printf ("CHAI3D\n");
printf ("Demo: 04-shapes\n");
printf ("Copyright 2003-2012\n");
printf ("-----------------------------------\n");
printf ("\n\n");
printf ("Keyboard Options:\n\n");
printf ("[x] - Exit application\n");
printf ("\n\n>\r");
// parse first arg to try and locate resources
resourceRoot = string(argv[0]).substr(0,string(argv[0]).find_last_of("/\\")+1);
//-----------------------------------------------------------------------
// WORLD - CAMERA - LIGHTING
//-----------------------------------------------------------------------
// create a new world.
world = new cWorld();
// set the background color of the environment
world->m_backgroundColor.setBlack();
// create a camera and insert it into the virtual world
camera = new cCamera(world);
world->addChild(camera);
// position and oriente the camera
camera->set( cVector3d (3.0, 0.0, 0.0), // camera position (eye)
cVector3d (0.0, 0.0, 0.0), // lookat position (target)
cVector3d (0.0, 0.0, 1.0)); // direction of the (up) vector
/* camera->set( cVector3d (0.0, 0.0, -3.0), // camera position (eye)
cVector3d (0.0, 0.0, 0.0), // lookat position (target)
cVector3d (-1.0, 0.0, 0.0)); // direction of the (up) vector*/
// set the near and far clipping planes of the camera
// anything in front/behind these clipping planes will not be rendered
camera->setClippingPlanes(0.01, 10.0);
// enable multi-pass rendering to handle transparent objects
camera->setUseMultipassTransparency(true);
// create a light source
light = new cDirectionalLight(world);
// add light to world
world->addChild(light);
// enable light source
light->setEnabled(true);
// define the direction of the light beam
light->setDir(-1.0, 0.0, 0.0);
//-----------------------------------------------------------------------
// HAPTIC DEVICES / TOOLS
//-----------------------------------------------------------------------
// create a haptic device handler
handler = new cHapticDeviceHandler();
// get access to the first available haptic device
handler->getDevice(hapticDevice, 0);
// retrieve information about the current haptic device
cHapticDeviceInfo hapticDeviceInfo = hapticDevice->getSpecifications();
// create a 3D tool and add it to the world
tool = new cToolCursor(world);
world->addChild(tool);
// connect the haptic device to the tool
tool->setHapticDevice(hapticDevice);
// initialize tool by connecting to haptic device
tool->start();
// map the physical workspace of the haptic device to a larger virtual workspace.
tool->setWorkspaceRadius(2.0);
// define a radius for the tool
tool->setRadius(0.03);
// read the scale factor between the physical workspace of the haptic
// device and the virtual workspace defined for the tool
double workspaceScaleFactor = tool->getWorkspaceScaleFactor();
// my device open **********************MINE
deltaCtrl.ConnectDevice();
//double R[3][3] = {0,0,-1,0,-1,0,-1,0,0};
//deltaCtrl.SetRotation(R);
//-----------------------------------------------------------------------
// CREATING OBJECTS
//-----------------------------------------------------------------------
// temp variable
cGenericEffect* newEffect;
// stiffness properties
double maxStiffness = hapticDeviceInfo.m_maxLinearStiffness / workspaceScaleFactor;
double maxLinearForce = hapticDeviceInfo.m_maxLinearForce;
// pull up stiffness
maxStiffness =300;
// create a sphere 0
sphere0 = new cShapeSphere(0.1);
world->addChild(sphere0);
sphere0->setLocalPos(0.0,-0.7, 0.0);
sphere0->m_material->setRedFireBrick();
newEffect = new cEffectSurface(sphere0);
sphere0->addEffect(newEffect);
sphere0->m_material->setStiffness(0.4 * maxStiffness);
// create a sphere 1
sphere1 = new cShapeSphere(0.1);
world->addChild(sphere1);
sphere1->setLocalPos(0.0, 0.7, 0.0);
sphere1->m_material->setRedFireBrick();
newEffect = new cEffectSurface(sphere1);
sphere1->addEffect(newEffect);
sphere1->m_material->setStiffness(0.4 * maxStiffness);
// create a line
line = new cShapeLine(sphere0->getLocalPos(), sphere1->getLocalPos());
world->addChild(line);
line->m_material->setWhite();
newEffect = new cEffectMagnet(line);
line->addEffect(newEffect);
line->m_material->setMagnetMaxDistance(0.05);
line->m_material->setMagnetMaxForce(0.1 * maxLinearForce);
line->m_material->setStiffness(0.1 * maxStiffness);
// create a cylinder
cylinder = new cShapeCylinder(0.25, 0.25, 0.2);
world->addChild(cylinder);
cylinder->setLocalPos(0.0, 0.0, 0.0);
cylinder->rotateAboutGlobalAxisDeg(cVector3d(1.0, 0.0, 0.0), 90);
cylinder->m_material->setBlueCornflower();
newEffect = new cEffectSurface(cylinder);
cylinder->addEffect(newEffect);
cylinder->m_material->setStiffness(0.8 * maxStiffness);
//-----------------------------------------------------------------------
// WIDGETS
//-----------------------------------------------------------------------
// create a font
cFont *font = NEW_CFONTCALIBRI20();
// create a label to display the haptic rate of the simulation
labelHapticRate = new cLabel(font);
camera->m_frontLayer->addChild(labelHapticRate);
//-----------------------------------------------------------------------
// OPEN GL - WINDOW DISPLAY
//-----------------------------------------------------------------------
// simulation in now running!
simulationRunning = true;
// initialize GLUT
glutInit(&argc, argv);
// retrieve the resolution of the computer display and estimate the position
// of the GLUT window so that it is located at the center of the screen
int screenW = glutGet(GLUT_SCREEN_WIDTH);
int screenH = glutGet(GLUT_SCREEN_HEIGHT);
int windowPosX = (screenW - WINDOW_SIZE_W) / 2;
int windowPosY = (screenH - WINDOW_SIZE_H) / 2;
// initialize the OpenGL GLUT window
glutInitWindowPosition(windowPosX, windowPosY);
glutInitWindowSize(WINDOW_SIZE_W, WINDOW_SIZE_H);
if (USE_STEREO_DISPLAY)
{
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE | GLUT_STEREO);
camera->setUseStereo(true);
}
else
{
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
camera->setUseStereo(false);
}
glutCreateWindow(argv[0]);
glutDisplayFunc(updateGraphics);
glutKeyboardFunc(keySelect);
glutReshapeFunc(resizeWindow);
glutSetWindowTitle("CHAI3D");
// create a mouse menu (right button)
glutCreateMenu(menuSelect);
glutAddMenuEntry("full screen", OPTION_FULLSCREEN);
glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);
glutAttachMenu(GLUT_RIGHT_BUTTON);
//-----------------------------------------------------------------------
// START SIMULATION
//-----------------------------------------------------------------------
// create a thread which starts the main haptics rendering loop
cThread* hapticsThread = new cThread();
hapticsThread->start(updateHaptics, CTHREAD_PRIORITY_HAPTICS);
// start the main graphics rendering loop
glutTimerFunc(30, graphicsTimer, 0);
glutMainLoop();
// close everything
close();
// exit
return (0);
}
int main(int argc, char* argv[])
{
// display pretty message
printf ("\n");
printf (" ===================================\n");
printf (" CHAI 3D\n");
printf (" Viewmesh Demo\n");
printf (" Copyright 2006\n");
printf ("\n Use the left mouse button to rotate the model\n");
printf ("\n Use the middle mouse button to move the model\n");
printf ("\n Use the right mouse button to switch to fullscreen\n");
printf (" ===================================\n");
printf ("\n");
// make sure the user specified a mesh
if (argc < 2)
{
printf("Usage: %s [mesh_filename]\n\n",argv[0]);
return -1;
}
// create a new world
world = new cWorld();
loadMesh(argv[1]);
if (object == 0) {
printf("Could not load model %s\n\n",argv[1]);
return -1;
}
// set background color
world->setBackgroundColor(0.0f,0.0f,0.0f);
// create a camera
camera = new cCamera(world);
world->addChild(camera);
// set camera position and orientation
//
// We choose to put it out on the positive z axis, so things appear
// the way OpenGL users expect them to appear, with z going in and
// out of the plane of the screen.
int result = camera->set(
cVector3d(0,0,4), // position of camera
cVector3d(0.0, 0.0, 0.0), // camera looking at origin
cVector3d(0.0, 1.0, 0.0) // orientation of camera (standing up)
);
// load a little chai bitmap logo which will located at the bottom of the screen
logo = new cBitmap();
logo->m_image.loadFromFile("./resources/images/chai3d.bmp");
logo->setPos(10,10,0);
camera->m_front_2Dscene.addChild(logo);
// we replace the background color of the logo (black) with a transparent color.
// we also enable transparency
logo->m_image.replace(cColorb(0,0,0), cColorb(0,0,0,0));
logo->enableTransparency(true);
// Create a light source and attach it to the camera
light = new cLight(world);
light->setEnabled(true);
light->setPos(cVector3d(2,0.5,1));
light->setDir(cVector3d(-2,0.5,1));
camera->addChild(light);
// create a tool and add it to the world.
tool = new cMeta3dofPointer(world, false);
// This is what we would do if we _didn't_ want the tool to
// move around as a child of the camera
// world->addChild(tool);
// Rotate the tool so its axes align with our opengl-like axes
// tool->rotate(cVector3d(0,0,1),-90.0*M_PI/180.0);
// tool->rotate(cVector3d(1,0,0),-90.0*M_PI/180.0);
// set up a nice-looking workspace for the phantom so
// it fits nicely with our models
tool->setPos(-4.0, 0.0, 0.0);
tool->setWorkspace(2.0,2.0,2.0);
tool->setRadius(0.05);
camera->addChild(tool);
// initialize the GLUT windows
glutInit(&argc, argv);
glutInitWindowSize(512, 512);
glutInitWindowPosition(0, 0);
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
glutCreateWindow(argv[0]);
glutDisplayFunc(draw);
glutKeyboardFunc(key);
glutReshapeFunc(resizeWindow);
glutSetWindowTitle("CHAI 3D");
// create a mouse menu
glutCreateMenu(menuCallback);
glutAddMenuEntry("Full Screen", OPTION_FULLSCREEN);
glutAddMenuEntry("Window Display", OPTION_WINDOWDISPLAY);
glutAttachMenu(GLUT_RIGHT_BUTTON);
glutMotionFunc(mouseMove);
glutMouseFunc(mouseDown);
// Make sure the haptic device knows where he is in the camera frame
world->computeGlobalPositions(true);
// set up the device
tool->initialize();
// open communication to the device
tool->start();
// start haptic timer callback
timer.set(0, hapticsLoop, NULL);
// update display
glutTimerFunc(30, updateDisplay, 0);
// start main graphic rendering loop
glutMainLoop();
return 0;
}
BOOL Crecord_playerApp::InitInstance() {
AfxEnableControlContainer();
#ifdef _AFXDLL
Enable3dControls(); // Call this when using MFC in a shared DLL
#else
Enable3dControlsStatic(); // Call this when linking to MFC statically
#endif
g_main_dlg = new Crecord_playerDlg;
m_pMainWnd = g_main_dlg;
g_main_dlg->Create(IDD_record_player_DIALOG,NULL);
// create a new world
world = new cWorld();
// set background color
world->setBackgroundColor(0.0f,0.0f,0.0f);
// Create a camera
camera = new cCamera(world);
world->addChild(camera);
// Create a light source and attach it to camera
light = new cLight(world);
light->setEnabled(true);
light->setPos(cVector3d(2,1,1));
// define camera position
flagCameraInMotion = false;
camera->set(cVector3d(2,0,1), cVector3d(0,0,0), cVector3d(0,0,1));
// create a display for graphic rendering
viewport = new cViewport(g_main_dlg->m_gl_area_hwnd, camera, true);
viewport->setStereoOn(false);
// init var
m_rotPos = 0;
m_rotVel = 0;
m_inertia = 0.04;
m_clock.initialize();
m_lastGoodPosition = 0.0;
m_reduction = 5.4*10;
m_cpt = 120*4;
m_desiredPos = 0.0;
m_P = 0.4;
m_I = 0.0014;
m_integratorVal = 0.0;
m_D = 0.02;
m_lastAngle = 0.0;
m_velocity = 0;
m_velocityOld = 0;
m_RFDInitialAngle = 0.0;
m_inContact = false;
LoadModel(TURNTABLE_MODEL);
object->translate(0,0,-0.3);
// set stiffness
double stiffness = (double)35;
object->setStiffness(stiffness, true);
// Initialize sound device and create audio stream
if (!BASS_Init(1,44100,0,0,NULL))
_cprintf("Init error %d\n", BASS_ErrorGetCode());
// Load a record onto the record player
load_record(0);
object->computeGlobalPositions(false);
world->computeGlobalPositions(false);
return TRUE;
}
//==============================================================================
void cDirectionalLight::setDir(const double a_x, const double a_y, const double a_z)
{
setDir(cVector3d(a_x, a_y, a_z));
}
void __fastcall TForm1::MassSpringFormCreate(TObject *Sender)
{
m_floor = 0;
m_floor_spring_constant = DEFAULT_FLOOR_SPRING_CONSTANT;
// init world scale
scale = 2.0;
// create a world
world = new cWorld();
// set background properties
world->setBackgroundColor(0.2,0.2,0.2);
// create a camera in world
camera = new cCamera(world);
world->addChild(camera);
// set camera position and orientation
camera->set( cVector3d(0.0, 0.0, 7.0), // position of camera.
cVector3d(0.0, 0.0, 0.0), // camera looking at origin.
cVector3d(0.0, 1.0, 0.0)); // orientation of camera. (standing up.)
// Create a light source and attach it to camera
light = new cLight(world);
camera->addChild(light);
light->setEnabled(true);
light->setPos(cVector3d(0,2,4));
light->rotate(cVector3d(0,0,1), cDegToRad(180));
// create a display for graphic rendering
viewport = new cViewport(Panel1->Handle, camera, false);
// create a tool
tool = new cMeta3dofPointer(world, 0, true);
tool->initialize();
tool->setRenderingMode(RENDER_DEVICE);
// Rotate the tool so its axes align with our opengl-like axes
tool->rotate(cVector3d(0,0,1),-90.0*M_PI/180.0);
tool->rotate(cVector3d(1,0,0),-90.0*M_PI/180.0);
tool->computeGlobalPositions();
tool->setWorkspace(3.5*scale, 3.5*scale, 3.5*scale);
tool->setRadius(scale / 70.0);
// tool becomes a child of the camera which we can control from
world->addChild(tool);
// Create a mesh to represent the floor
m_floor = new cMesh(world);
world->addChild(m_floor);
// Fill in meaningful vertex positions
m_floor->newVertex(-FLOOR_X_SIZE/2.0, FLOOR_Y_POSITION, -FLOOR_Z_SIZE/2.0);
m_floor->newVertex(-FLOOR_X_SIZE/2.0, FLOOR_Y_POSITION, FLOOR_Z_SIZE/2.0);
m_floor->newVertex( FLOOR_X_SIZE/2.0, FLOOR_Y_POSITION, FLOOR_Z_SIZE/2.0);
m_floor->newTriangle(0,1,2);
m_floor->newVertex( FLOOR_X_SIZE/2.0, FLOOR_Y_POSITION, -FLOOR_Z_SIZE/2.0);
m_floor->newVertex(-FLOOR_X_SIZE/2.0, FLOOR_Y_POSITION, -FLOOR_Z_SIZE/2.0);
m_floor->newVertex( FLOOR_X_SIZE/2.0, FLOOR_Y_POSITION, FLOOR_Z_SIZE/2.0);
m_floor->newTriangle(3,4,5);
for(int n=0; n<6; n++)
{
cVertex* curVertex = m_floor->getVertex(n);
curVertex->setNormal(0,1,0);
}
// Give him some material properties...
cMaterial material;
material.m_ambient.set( 0.2, 0.2, 0.2, 1.0 );
material.m_diffuse.set( 0.6, 0.6, 0.6, 1.0 );
material.m_specular.set( 0.9, 0.9, 0.9, 1.0 );
material.setShininess(100);
m_floor->m_material = material;
// Create an initial ball
CBall* b = new CBall();
m_active_balls.push_back(b);
cVector3d pos(-1.0,0,1.0);
b->setPos(pos);
world->addChild(b);
// Create a series of masses connected by springs
for(int i=1; i<INITIAL_NUM_BALLS; i++) {
add_ball();
}
simulationOn = false;
}
bool ch_proxyPointForceAlgo::computeNextProxyPositionWithContraints22(const cVector3d& a_goalGlobalPos, const cVector3d& a_toolVel)
{
// The proxy is now constrained by two triangles and can only move along
// a virtual line; we now calculate the nearest point to the original
// goal (device position) along this line by projecting the ideal
// goal onto the line.
//
// The line is expressed by the cross product of both surface normals,
// which have both been oriented to point away from the device
cVector3d line;
m_collisionRecorderConstraint0.m_nearestCollision.m_globalNormal.crossr(m_collisionRecorderConstraint1.m_nearestCollision.m_globalNormal, line);
// check result.
if (line.equals(cVector3d(0,0,0)))
{
m_nextBestProxyGlobalPos = m_proxyGlobalPos;
m_algoCounter = 0;
m_numContacts = 2;
return (false);
}
line.normalize();
// Compute the projection of the device position (goal) onto the line; this
// gives us the new goal position.
cVector3d goalGlobalPos = cProjectPointOnLine(a_goalGlobalPos, m_proxyGlobalPos, line);
// A vector from the proxy to the goal
cVector3d vProxyToGoal;
goalGlobalPos.subr(m_proxyGlobalPos, vProxyToGoal);
// If the distance between the proxy and the goal position (device) is
// very small then we can be considered done.
if (goalAchieved(m_proxyGlobalPos, goalGlobalPos))
{
m_nextBestProxyGlobalPos = m_proxyGlobalPos;
m_algoCounter = 0;
m_numContacts = 2;
return (false);
}
// compute the normalized form of the vector going from the
// current proxy position to the desired goal position
cVector3d vProxyToGoalNormalized;
vProxyToGoal.normalizer(vProxyToGoalNormalized);
// Test whether the path from the proxy to the goal is obstructed.
// For this we create a segment that goes from the proxy position to
// the goal position plus a little extra to take into account the
// physical radius of the proxy.
cVector3d targetPos = goalGlobalPos +
cMul(m_epsilonCollisionDetection, vProxyToGoalNormalized);
// setup collision detector
m_collisionSettings.m_collisionRadius = m_radius;
// search for collision
m_collisionSettings.m_adjustObjectMotion = false;
m_collisionRecorderConstraint2.clear();
bool hit = m_world->computeCollisionDetection( m_proxyGlobalPos,
targetPos,
m_collisionRecorderConstraint2,
m_collisionSettings);
// check if collision occurred between proxy and goal positions.
double collisionDistance;
if (hit)
{
collisionDistance = sqrt(m_collisionRecorderConstraint2.m_nearestCollision.m_squareDistance);
if (collisionDistance > (cDistance(m_proxyGlobalPos, goalGlobalPos) + CHAI_SMALL))
{
hit = false;
}
else
{
// a collision has occurred and we check if the distance from the
// proxy to the collision is smaller than epsilon. If yes, then
// we reduce the epsilon term in order to avoid possible "pop through"
// effect if we suddenly push the proxy "up" again.
if (collisionDistance < m_epsilon)
{
m_epsilon = collisionDistance;
if (m_epsilon < m_epsilonMinimalValue)
{
m_epsilon = m_epsilonMinimalValue;
}
}
}
}
// If no collision occurs, we move the proxy to its goal, unless
// friction prevents us from doing so
if (!hit)
{
cVector3d normal = cMul(0.5,cAdd(m_collisionRecorderConstraint0.m_nearestCollision.m_globalNormal,
m_collisionRecorderConstraint1.m_nearestCollision.m_globalNormal));
testFrictionAndMoveProxy(goalGlobalPos,
m_proxyGlobalPos,
normal,
m_collisionRecorderConstraint1.m_nearestCollision.m_triangle->getParent(), a_toolVel);
m_numContacts = 2;
m_algoCounter = 0;
return (false);
}
//-----------------------------------------------------------------------
// THIRD COLLISION OCCURES:
//-----------------------------------------------------------------------
// We want the center of the proxy to move as far toward the triangle as it can,
// but we want it to stop when the _sphere_ representing the proxy hits the
// triangle. We want to compute how far the proxy center will have to
// be pushed _away_ from the collision point - along the vector from the proxy
// to the goal - to keep a distance m_radius between the proxy center and the
// triangle.
//
// So we compute the cosine of the angle between the normal and proxy-goal vector...
double cosAngle = vProxyToGoalNormalized.dot(m_collisionRecorderConstraint2.m_nearestCollision.m_globalNormal);
// Now we compute how far away from the collision point - _backwards_
// along vProxyGoal - we have to put the proxy to keep it from penetrating
// the triangle.
//
// If only ASCII art were a little more expressive...
double distanceTriangleProxy = m_epsilon / cAbs(cosAngle);
if (distanceTriangleProxy > collisionDistance) {
distanceTriangleProxy = cMax(collisionDistance, m_epsilon);
}
// We compute the projection of the vector between the proxy and the collision
// point onto the normal of the triangle. This is the direction in which
// we'll move the _goal_ to "push it away" from the triangle (to account for
// the radius of the proxy).
// A vector from the most recent collision point to the proxy
cVector3d vCollisionToProxy;
m_proxyGlobalPos.subr(m_contactPoint2->m_globalPos, vCollisionToProxy);
// Move the proxy to the collision point, minus the distance along the
// movement vector that we computed above.
//
// Note that we're adjusting the 'proxy' variable, which is just a local
// copy of the proxy position. We still might decide not to move the
// 'real' proxy due to friction.
cVector3d vColNextGoal;
vProxyToGoalNormalized.mulr(-distanceTriangleProxy, vColNextGoal);
cVector3d nextProxyPos;
m_contactPoint2->m_globalPos.addr(vColNextGoal, nextProxyPos);
// we can now set the next position of the proxy
m_nextBestProxyGlobalPos = nextProxyPos;
m_algoCounter = 0;
m_numContacts = 3;
// TODO: There actually should be a third friction test to see if we
// can make it to our new goal position, but this is generally such a
// small movement in one iteration that it's irrelevant...
return (true);
}
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
printf ("\n");
printf ("-----------------------------------\n");
printf ("CHAI 3D\n");
printf ("Demo: 52-GEL-duck\n");
printf ("Copyright 2003-2010\n");
printf ("-----------------------------------\n");
printf ("\n\n");
printf ("Keyboard Options:\n\n");
printf ("[1] - Show GEL Skeleton\n");
printf ("[2] - Hide GEL Skeleton\n");
printf ("[x] - Exit application\n");
printf ("\n\n");
// parse first arg to try and locate resources
resourceRoot = string(argv[0]).substr(0,string(argv[0]).find_last_of("/\\")+1);
//-----------------------------------------------------------------------
// 3D - SCENEGRAPH
//-----------------------------------------------------------------------
// create a new world.
world = new cWorld();
// set the background color of the environment
// the color is defined by its (R,G,B) components.
world->setBackgroundColor(0.0, 0.0, 0.0);
world->setBackgroundColor(1.0, 1.0, 1.0);
// create a camera and insert it into the virtual world
camera = new cCamera(world);
world->addChild(camera);
// position and oriente the camera
// define a default position of the camera (described in spherical coordinates)
cameraAngleH = 0;
cameraAngleV = 45;
cameraDistance = 2.5;
updateCameraPosition();
// set the near and far clipping planes of the camera
// anything in front/behind these clipping planes will not be rendered
camera->setClippingPlanes(0.01, 10.0);
// enable higher rendering quality because we are displaying transparent objects
camera->enableMultipassTransparency(true);
// create a light source and attach it to the camera
light = new cLight(world);
camera->addChild(light); // attach light to camera
light->setEnabled(true); // enable light source
light->setPos(cVector3d( 2.0, 0.5, 1.0)); // position the light source
light->setDir(cVector3d(-2.0, 0.5, 1.0)); // define the direction of the light beam
light->setDirectionalLight(false);
light->m_ambient.set(0.5, 0.5, 0.5);
light->m_diffuse.set(0.7, 0.7, 0.7);
light->m_specular.set(0.9, 0.9, 0.9);
//-----------------------------------------------------------------------
// 2D - WIDGETS
//-----------------------------------------------------------------------
// create a 2D bitmap logo
logo = new cBitmap();
// add logo to the front plane
camera->m_front_2Dscene.addChild(logo);
// load a "chai3d" bitmap image file
bool fileload;
fileload = logo->m_image.loadFromFile(RESOURCE_PATH("resources/images/chai3d.bmp"));
if (!fileload)
{
#if defined(_MSVC)
fileload = logo->m_image.loadFromFile("../../../bin/resources/images/chai3d.bmp");
#endif
}
// position the logo at the bottom left of the screen (pixel coordinates)
logo->setPos(10, 10, 0);
// scale the logo along its horizontal and vertical axis
logo->setZoomHV(0.4, 0.4);
// here we replace all black pixels (0,0,0) of the logo bitmap
// with transparent black pixels (0, 0, 0, 0). This allows us to make
// the background of the logo look transparent.
logo->m_image.replace(
cColorb(0, 0, 0), // original RGB color
cColorb(0, 0, 0, 0) // new RGBA color
);
// enable transparency
logo->enableTransparency(true);
//-----------------------------------------------------------------------
// HAPTIC DEVICES / TOOLS
//-----------------------------------------------------------------------
// create a haptic device handler
handler = new cHapticDeviceHandler();
// get access to the first available haptic device
handler->getDevice(hapticDevice, 0);
// retrieve information about the current haptic device
cHapticDeviceInfo info;
if (hapticDevice)
{
hapticDevice->open();
info = hapticDevice->getSpecifications();
}
// desired workspace radius of the cursor
cursorWorkspaceRadius = 0.8;
// read the scale factor between the physical workspace of the haptic
// device and the virtual workspace defined for the tool
workspaceScaleFactor = cursorWorkspaceRadius / info.m_workspaceRadius;
// define a scale factor between the force perceived at the cursor and the
// forces actually sent to the haptic device
deviceForceScale = 0.005 * (info.m_maxForceStiffness / workspaceScaleFactor);
// define a force scale factor for the current haptic device.
const double FORCE_REFERENCE = 10.0;
deviceForceScale = info.m_maxForce / FORCE_REFERENCE;
// create a large sphere that represents the haptic device
deviceRadius = 0.12;
device = new cShapeSphere(deviceRadius);
world->addChild(device);
device->m_material.m_ambient.set(0.4, 0.4, 0.4, 0.7);
device->m_material.m_diffuse.set(0.7, 0.7, 0.7, 0.7);
device->m_material.m_specular.set(1.0, 1.0, 1.0, 0.7);
device->m_material.setShininess(100);
stiffness = 40;
//-----------------------------------------------------------------------
// COMPOSE THE VIRTUAL SCENE
//-----------------------------------------------------------------------
// create a world which supports deformable object
defWorld = new cGELWorld();
world->addChild(defWorld);
world->setPos(-1.5, 0.0, -0.1);
world->rotate(cVector3d(0,1,0), cDegToRad(30));
//-----------------------------------------------------------------------
// COMPOSE THE WATER
//-----------------------------------------------------------------------
ground = new cGELMesh(world);
ground->m_useMassParticleModel = true;
defWorld->m_gelMeshes.push_back(ground);
cGELMassParticle::default_mass = 0.010;
cGELMassParticle::default_kDampingPos = 0.4;
cGELMassParticle::default_gravity.set(0,0,0);
int u,v;
int RESOLUTION = 15;
double SIZE = 3.5;
for (v=0; v<RESOLUTION; v++)
{
for (u=0; u<RESOLUTION; u++)
{
double px, py, tu, tv;
// compute the position of the vertex
px = SIZE / (double)RESOLUTION * (double)u - (SIZE/2.0);
py = SIZE / (double)RESOLUTION * (double)v - (SIZE/2.0);
// create new vertex
unsigned int index = ground->newVertex(px, py, level);
cVertex* vertex = ground->getVertex(index);
// compute texture coordinate
tu = (double)u / (double)RESOLUTION;
tv = (double)v / (double)RESOLUTION;
vertex->setTexCoord(tu, tv);
vertex->setColor(cColorf(1.0, 0.0, 0.1));
}
}
ground->buildVertices();
for (v=0; v<RESOLUTION; v++)
{
for (u=0; u<RESOLUTION; u++)
{
if ((u == 0) || (v == 0) || (u == (RESOLUTION-1)) || (v == (RESOLUTION-1)))
{
// create new vertex
unsigned int index = ((v + 0) * RESOLUTION) + (u + 0);
ground->m_gelVertices[index].m_massParticle->m_fixed = true;
}
}
}
cGELLinearSpring::default_kSpringElongation = 10.0; // [N/m]
// Create a triangle based map using the above pixels
for (v=0; v<(RESOLUTION-1); v++)
{
for (u=0; u<(RESOLUTION-1); u++)
{
// get the indexing numbers of the next four vertices
unsigned int index00 = ((v + 0) * RESOLUTION) + (u + 0);
unsigned int index01 = ((v + 0) * RESOLUTION) + (u + 1);
unsigned int index10 = ((v + 1) * RESOLUTION) + (u + 0);
unsigned int index11 = ((v + 1) * RESOLUTION) + (u + 1);
// create two new triangles
ground->newTriangle(index00, index01, index10);
ground->newTriangle(index10, index01, index11);
cGELMassParticle* m0 = ground->m_gelVertices[index00].m_massParticle;
cGELMassParticle* m1 = ground->m_gelVertices[index01].m_massParticle;
cGELMassParticle* m2 = ground->m_gelVertices[index10].m_massParticle;
cGELLinearSpring* spring0 = new cGELLinearSpring(m0, m1);
cGELLinearSpring* spring1 = new cGELLinearSpring(m0, m2);
ground->m_linearSprings.push_back(spring0);
ground->m_linearSprings.push_back(spring1);
}
}
double transparencyLevel = 0.7;
ground->setUseMaterial(true);
ground->m_material.m_ambient.set(0.6, 0.6, 0.6, transparencyLevel);
ground->m_material.m_diffuse.set(0.8, 0.8, 0.8, transparencyLevel);
ground->m_material.m_specular.set(0.9, 0.9, 0.9, transparencyLevel);
ground->setTransparencyLevel(transparencyLevel);
ground->setUseTransparency(true);
cTexture2D* textureGround = new cTexture2D();
ground->setTexture(textureGround);
ground->setUseTexture(true, true);
fileload = textureGround->loadFromFile(RESOURCE_PATH("resources/images/aqua.bmp"));
if (!fileload)
{
#if defined(_MSVC)
fileload = textureGround->loadFromFile("../../../bin/resources/images/aqua.bmp" );
#endif
if (!fileload)
{
printf("Error - 3D Texture failed to load correctly.\n");
close();
return (-1);
}
}
//-----------------------------------------------------------------------
// COMPOSE SOME REFLEXION
//-----------------------------------------------------------------------
cGenericObject* reflexion = new cGenericObject();
world->addChild(reflexion);
cMatrix3d rot;
rot.set(1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0);
reflexion->setRot(rot);
reflexion->addChild(defWorld);
reflexion->setPos(0.0, 0.0, 2.0 * level);
cGenericObject* reflexionTool = new cGenericObject();
reflexion->addChild(reflexionTool);
reflexionTool->addChild(device);
reflexionTool->setPos(0.0, 0.0, -0.5 * level);
// Play with thse numbers carefully!
defWorld->m_integrationTime = 0.001;
// create a deformable mesh
defObject = new cGELMesh(world);
defWorld->m_gelMeshes.push_front(defObject);
//-----------------------------------------------------------------------
// BUILD THE DUCK!
//-----------------------------------------------------------------------
if (USE_SKELETON_MODEL)
{
fileload = createSkeletonMesh(defObject, RESOURCE_PATH("resources/models/ducky/duck-200.off"), RESOURCE_PATH("resources/models/ducky/duck-full.obj"));
if (!fileload)
{
#if defined(_MSVC)
fileload = createSkeletonMesh(defObject, "../../../bin/resources/models/ducky/duck-200.off", "../../../bin/resources/models/ducky/duck-full.obj");
#endif
if (!fileload)
{
printf("Error - 3D Model failed to load correctly.\n");
close();
return (-1);
}
}
}
else
{
fileload = createTetGenMesh(defObject, RESOURCE_PATH("resources/models/ducky/duck-1200.off"), RESOURCE_PATH("resources/models/ducky/duck-green.obj"));
if (!fileload)
{
#if defined(_MSVC)
fileload = createTetGenMesh(defObject, "../../../bin/resources/models/ducky/duck-1200.off", "../../../bin/resources/models/ducky/duck-green.obj");
#endif
if (!fileload)
{
printf("Error - 3D Model failed to load correctly.\n");
close();
return (-1);
}
}
}
//-----------------------------------------------------------------------
// OPEN GL - WINDOW DISPLAY
//-----------------------------------------------------------------------
// initialize GLUT
glutInit(&argc, argv);
// retrieve the resolution of the computer display and estimate the position
// of the GLUT window so that it is located at the center of the screen
int screenW = glutGet(GLUT_SCREEN_WIDTH);
int screenH = glutGet(GLUT_SCREEN_HEIGHT);
int windowPosX = (screenW - WINDOW_SIZE_W) / 2;
int windowPosY = (screenH - WINDOW_SIZE_H) / 2;
// initialize the OpenGL GLUT window
glutInitWindowPosition(windowPosX, windowPosY);
glutInitWindowSize(WINDOW_SIZE_W, WINDOW_SIZE_H);
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
glutCreateWindow(argv[0]);
glutMouseFunc(mouseClick);
glutMotionFunc(mouseMove);
glutDisplayFunc(updateGraphics);
glutKeyboardFunc(keySelect);
glutReshapeFunc(resizeWindow);
glutSetWindowTitle("CS 268 - Sonny Chan & Francois Conti - March 2009");
// create a mouse menu (right button)
glutCreateMenu(menuSelect);
glutAddMenuEntry("full screen", OPTION_FULLSCREEN);
glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);
glutAddMenuEntry("show skeleton", OPTION_SHOWSKELETON);
glutAddMenuEntry("hide skeleton", OPTION_HIDESKELETON);
glutAttachMenu(GLUT_RIGHT_BUTTON);
//-----------------------------------------------------------------------
// START SIMULATION
//-----------------------------------------------------------------------
// simulation in now running
simulationRunning = true;
// create a thread which starts the main haptics rendering loop
cThread* hapticsThread = new cThread();
hapticsThread->set(updateHaptics, CHAI_THREAD_PRIORITY_HAPTICS);
// start the main graphics rendering loop
glutMainLoop();
// close everything
close();
// exit
return (0);
}
void updateHaptics(void)
{
// reset simulation clock
simClock.reset();
simClock.start();
// main haptic simulation loop
while(simulationRunning)
{
// read position from haptic device
cVector3d pos;
hapticDevice->getPosition(pos);
pos.mul(workspaceScaleFactor);
device->setPos(pos);
// init temp variable
cVector3d force;
force.zero();
// compute reaction forces
list<cGELMesh*>::iterator i;
// model water level
for(i = defWorld->m_gelMeshes.begin(); i != defWorld->m_gelMeshes.end(); ++i)
{
cGELMesh *nextItem = *i;
if (nextItem->m_useMassParticleModel)
{
int numVertices = nextItem->m_gelVertices.size();
for (int i=0; i<numVertices; i++)
{
cVector3d nodePos = nextItem->m_gelVertices[i].m_massParticle->m_pos;
cVector3d force = cVector3d(-0.002 * nodePos.x, -0.002 * nodePos.y, 0.0);
if (nodePos.z < level)
{
double depth = nodePos.z - level;
force.add(cVector3d(0,0,-100*depth));
}
nextItem->m_gelVertices[i].m_massParticle->setExternalForce(force);
}
}
if (nextItem->m_useSkeletonModel)
{
list<cGELSkeletonNode*>::iterator i;
for(i = nextItem->m_nodes.begin(); i != nextItem->m_nodes.end(); ++i)
{
cGELSkeletonNode* node = *i;
cVector3d nodePos = node->m_pos;
double radius = node->m_radius;
cVector3d force = cVector3d(-0.01 * nodePos.x, -0.01 * (nodePos.y), 0.0);
if ((nodePos.z-radius) < level)
{
double depth = (nodePos.z-radius) - level;
force.add(cVector3d(0,0,-1.0 * depth));
node->m_vel.mul(0.95);
}
node->setExternalForce(force);
}
}
}
// compute haptic feedback
for(i = defWorld->m_gelMeshes.begin(); i != defWorld->m_gelMeshes.end(); ++i)
{
cGELMesh *nextItem = *i;
if (nextItem->m_useMassParticleModel)
{
int numVertices = nextItem->m_gelVertices.size();
for (int i=0; i<numVertices; i++)
{
cVector3d nodePos = nextItem->m_gelVertices[i].m_massParticle->m_pos;
cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
if (f.lengthsq() > 0)
{
cVector3d tmpfrc = cNegate(f);
nextItem->m_gelVertices[i].m_massParticle->setExternalForce(tmpfrc);
}
force.add(cMul(1.0, f));
}
}
if (nextItem->m_useSkeletonModel)
{
list<cGELSkeletonNode*>::iterator i;
for(i = nextItem->m_nodes.begin(); i != nextItem->m_nodes.end(); ++i)
{
cGELSkeletonNode* node = *i;
cVector3d nodePos = node->m_pos;
double radius = node->m_radius;
cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
if (f.lengthsq() > 0)
{
cVector3d tmpfrc = cNegate(f);
node->setExternalForce(tmpfrc);
}
force.add(cMul(4.0, f));
}
}
}
// integrate dynamics
double interval = simClock.stop();
simClock.reset();
simClock.start();
if (interval > 0.001) { interval = 0.001; }
defWorld->updateDynamics(interval);
// scale force
force.mul(0.6 * deviceForceScale);
// water viscosity
if ((pos.z - deviceRadius) < level)
{
// read damping properties of haptic device
cHapticDeviceInfo info = hapticDevice->getSpecifications();
double Kv = 0.8 * info.m_maxLinearDamping;
// read device velocity
cVector3d linearVelocity;
hapticDevice->getLinearVelocity(linearVelocity);
// compute a scale factor [0,1] proportional to percentage
// of tool volume immersed in the water
double val = (level - (pos.z - deviceRadius)) / (2.0 * deviceRadius);
double scale = cClamp(val, 0.1, 1.0);
// compute force
cVector3d forceDamping = cMul(-Kv * scale, linearVelocity);
force.add(forceDamping);
}
// send forces to haptic device
hapticDevice->setForce(force);
}
// exit haptics thread
simulationFinished = true;
}
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
printf ("\n");
printf ("-----------------------------------\n");
printf ("CHAI3D\n");
printf ("Demo: 10-oring\n");
printf ("Copyright 2003-2012\n");
printf ("-----------------------------------\n");
printf ("\n\n");
printf ("Keyboard Options:\n\n");
printf ("[1] - Texture (ON/OFF)\n");
printf ("[2] - Wireframe (ON/OFF)\n");
printf ("[x] - Exit application\n");
printf ("\n\n>\r");
// parse first arg to try and locate resources
string resourceRoot = string(argv[0]).substr(0,string(argv[0]).find_last_of("/\\")+1);
//-----------------------------------------------------------------------
// WORLD - CAMERA - LIGHTING
//-----------------------------------------------------------------------
// create a new world.
world = new cWorld();
// set the background color of the environment
world->m_backgroundColor.setBlack();
// create a camera and insert it into the virtual world
camera = new cCamera(world);
world->addChild(camera);
// position and oriente the camera
camera->set( cVector3d (3.0, 0.0, 0.0), // camera position (eye)
cVector3d (0.0, 0.0, 0.0), // lookat position (target)
cVector3d (0.0, 0.0, 1.0)); // direction of the (up) vector
// set the near and far clipping planes of the camera
// anything in front/behind these clipping planes will not be rendered
camera->setClippingPlanes(0.01, 10.0);
// enable shadow casting
camera->setUseShadowCasting(true);
// create a light source
light = new cSpotLight(world);
// attach light to camera
camera->addChild(light);
// enable light source
light->setEnabled(true);
// position the light source
light->setLocalPos( 0.0, 0.5, 0.0);
// define the direction of the light beam
light->setDir(-3.0,-0.5, 0.0);
// enable this light source to generate shadows
light->setShadowMapEnabled(true);
//-----------------------------------------------------------------------
// HAPTIC DEVICES / TOOLS
//-----------------------------------------------------------------------
// create a haptic device handler
handler = new cHapticDeviceHandler();
// get access to the first available haptic device
handler->getDevice(hapticDevice, 0);
// retrieve information about the current haptic device
cHapticDeviceInfo info = hapticDevice->getSpecifications();
// create a 3D tool and add it to the world
tool = new cToolCursor(world);
world->addChild(tool);
// connect the haptic device to the tool
tool->setHapticDevice(hapticDevice);
// initialize tool by connecting to haptic device
tool->start();
// map the physical workspace of the haptic device to a larger virtual workspace.
tool->setWorkspaceRadius(1.0);
// define a radius for the tool
tool->setRadius(0.03);
// read the scale factor between the physical workspace of the haptic
// device and the virtual workspace defined for the tool
double workspaceScaleFactor = tool->getWorkspaceScaleFactor();
// define a maximum stiffness that can be handled by the current
// haptic device. The value is scaled to take into account the
// workspace scale factor
double stiffnessMax = info.m_maxLinearStiffness / workspaceScaleFactor;
//-----------------------------------------------------------------------
// CREATING OBJECTS
//-----------------------------------------------------------------------
// create a torus object (o-ring)
object = new cShapeTorus(0.24, 0.50);
// add object to world
world->addChild(object);
// set the position of the object at the center of the world
object->setLocalPos(0.0, 0.0, 0.0);
// rotate object of 90 degrees around its y-axis so that it
// stands vertically
object->rotateAboutGlobalAxisDeg( cVector3d(0, 1, 0), 90);
// set the material stiffness to 100% of the maximum permitted stiffness
// of the haptic device
object->m_material->setStiffness(1.00 * stiffnessMax);
// set some environmental texture and material properties
object->m_texture = new cTexture2d();
// load texture file
bool fileload = object->m_texture->loadFromFile(RESOURCE_PATH("resources/images/spheremap-6.jpg"));
if (!fileload)
{
//fileload = object->m_texture->loadFromFile("../../../bin/resources/images/spheremap-6.jpg");
fileload = object->m_texture->loadFromFile("resources/images/spheremap-6.jpg");
}
if (!fileload)
{
printf("Error - Texture image failed to load correctly.\n");
close();
return (-1);
}
// setup texture and material properties
object->setUseTexture(true);
object->m_texture->setSphericalMappingEnabled(true);
object->m_material->m_ambient.set(0.9, 0.9, 0.9);
object->m_material->m_diffuse.set(0.9, 0.9, 0.9);
object->m_material->m_specular.set(1.0, 1.0, 1.0);
// create a haptic effect for this object so that the operator can
// feel its surface
cEffectSurface* newEffect = new cEffectSurface(object);
object->addEffect(newEffect);
//-----------------------------------------------------------------------
// WIDGETS
//-----------------------------------------------------------------------
// create a font
cFont *font = NEW_CFONTCALIBRI20();
// create a label to display the haptic rate of the simulation
labelHapticRate = new cLabel(font);
camera->m_frontLayer->addChild(labelHapticRate);
level = new cLevel();
camera->m_frontLayer->addChild(level);
level->setLocalPos(100,100,0);
level->setRange(0.0, 25.0);
dial = new cDial();
camera->m_frontLayer->addChild(dial);
dial->setSingleIncrementDisplay(false);
dial->setLocalPos(600,100,0);
dial->setRange(0.0, 5.0);
dial->setSize(60);
dial->setValue0(0.0);
//-----------------------------------------------------------------------
// OPEN GL - WINDOW DISPLAY
//-----------------------------------------------------------------------
// simulation in now running!
simulationRunning = true;
// initialize GLUT
glutInit(&argc, argv);
// retrieve the resolution of the computer display and estimate the position
// of the GLUT window so that it is located at the center of the screen
int screenW = glutGet(GLUT_SCREEN_WIDTH);
int screenH = glutGet(GLUT_SCREEN_HEIGHT);
int windowPosX = (screenW - WINDOW_SIZE_W) / 2;
int windowPosY = (screenH - WINDOW_SIZE_H) / 2;
// initialize the OpenGL GLUT window
glutInitWindowPosition(windowPosX, windowPosY);
glutInitWindowSize(WINDOW_SIZE_W, WINDOW_SIZE_H);
if (USE_STEREO_DISPLAY)
{
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE | GLUT_STEREO);
camera->setUseStereo(true);
}
else
{
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
camera->setUseStereo(false);
}
glutCreateWindow(argv[0]);
glutDisplayFunc(updateGraphics);
glutKeyboardFunc(keySelect);
glutReshapeFunc(resizeWindow);
glutSetWindowTitle("CHAI3D");
// create a mouse menu (right button)
glutCreateMenu(menuSelect);
glutAddMenuEntry("full screen", OPTION_FULLSCREEN);
glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);
glutAttachMenu(GLUT_RIGHT_BUTTON);
//-----------------------------------------------------------------------
// START SIMULATION
//-----------------------------------------------------------------------
// create a thread which starts the main haptics rendering loop
cThread* hapticsThread = new cThread();
hapticsThread->start(updateHaptics, CTHREAD_PRIORITY_HAPTICS);
// start the main graphics rendering loop
glutTimerFunc(30, graphicsTimer, 0);
glutMainLoop();
// close everything
close();
// exit
return (0);
}
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
printf ("\n");
printf ("-----------------------------------\n");
printf ("CHAI3D\n");
printf ("Demo: 40-ODE-cubic\n");
printf ("Copyright 2003-2011\n");
printf ("-----------------------------------\n");
printf ("\n\n");
printf ("Instructions:\n\n");
printf ("- Use haptic device and user switch to manipulate cubes \n");
printf ("\n\n");
printf ("Keyboard Options:\n\n");
printf ("[1] - Enable gravity\n");
printf ("[2] - Disable gravity\n");
printf ("[x] - Exit application\n");
printf ("\n\n");
// parse first arg to try and locate resources
resourceRoot = string(argv[0]).substr(0,string(argv[0]).find_last_of("/\\")+1);
//-----------------------------------------------------------------------
// 3D - SCENEGRAPH
//-----------------------------------------------------------------------
// create a new world.
world = new cWorld();
// set the background color of the environment
// the color is defined by its (R,G,B) components.
// world->setBackgroundColor(0.0, 0.0, 0.0);
world->m_backgroundColor.setWhite();
// create a camera and insert it into the virtual world
camera = new cCamera(world);
world->addChild(camera);
// position and oriente the camera
camera->set( cVector3d (2.5, 0.0, 0.3), // camera position (eye)
cVector3d (0.0, 0.0, -0.5), // lookat position (target)
cVector3d (0.0, 0.0, 1.0)); // direction of the "up" vector
// set the near and far clipping planes of the camera
// anything in front/behind these clipping planes will not be rendered
camera->setClippingPlanes(0.1, 4.0);
camera->setUseShadowCasting(true);
camera->setUseMultipassTransparency(true);
// create a light source and attach it to the camera
light = new cSpotLight(world);
world->addChild(light); // attach light to camera
light->setEnabled(true); // enable light source
light->setLocalPos(cVector3d( 1.5, 0.5, 1.5)); // position the light source
light->setDir(cVector3d(-1.0, -0.5, -1.0)); // define the direction of the light beam
light->m_ambient.set(0.6, 0.6, 0.6);
light->m_diffuse.set(0.8, 0.8, 0.8);
light->m_specular.set(0.8, 0.8, 0.8);
light->m_shadowMap->setEnabled(true);
light->setCutOffAngleDeg(40);
// light->setDisplaySettings(0.05, 4.0, true);
//light->setDisplayEnabled(true);
//light->setShowFrame(true);
//light->setShowEnabled(true);
cDirectionalLight* light2 = new cDirectionalLight(world);
world->addChild(light2); // attach light to camera
light2->setEnabled(true); // enable light source
light2->setDir(cVector3d(-1.0, 0.0, -1.0)); // define the direction of the light beam
light2->m_ambient.set(0.3, 0.3, 0.3);
light2->m_diffuse.set(0.6, 0.6, 0.6);
light2->m_specular.set(0.0, 0.0, 0.0);
//-----------------------------------------------------------------------
// 2D - WIDGETS
//-----------------------------------------------------------------------
/*
// create a 2D bitmap logo
logo = new cBitmap();
// add logo to the front plane
camera->m_frontLayer->addChild(logo);
// load a "chai3d" bitmap image file
logo->setImage (NEW_CHAI3D_LOGO());
// position the logo at the bottom left of the screen (pixel coordinates)
logo->setLocalPos(10, 10, 0);
// scale the logo along its horizontal and vertical axis
logo->setZoom(0.25, 0.25);
// here we replace all black pixels (0,0,0) of the logo bitmap
// with transparent black pixels (0, 0, 0, 0). This allows us to make
// the background of the logo look transparent.
logo->m_image->setTransparentColor(0x00, 0x00, 0x00, 0x00);
// enable transparency
logo->setUseTransparency(true);
*/
//-----------------------------------------------------------------------
// HAPTIC DEVICES / TOOLS
//-----------------------------------------------------------------------
// create a haptic device handler
handler = new cHapticDeviceHandler();
// get access to the first available haptic device
cGenericHapticDevice* hapticDevice;
handler->getDevice(hapticDevice, 0);
// retrieve information about the current haptic device
cHapticDeviceInfo info;
if (hapticDevice)
{
info = hapticDevice->getSpecifications();
}
// create a 3D tool and add it to the world
tool = new cToolGripper(world);
// tool = new cToolCursor(world);
world->addChild(tool);
// connect the haptic device to the tool
tool->setHapticDevice(hapticDevice);
// initialize tool by connecting to haptic device
tool->start();
// map the physical workspace of the haptic device to a larger virtual workspace.
tool->setWorkspaceRadius(1.3);
// define a radius for the tool (graphical display)
tool->setRadius(0.03);
// hide the device sphere. only show proxy.
tool->setShowContactPoints(true, false);
// set the physical readius of the proxy.
proxyRadius = 0.03;
tool->setRadiusContact(proxyRadius);
// tool->m_proxyPointForceModel->m_collisionSettings.m_checkBothSidesOfTriangles = false;
// enable if objects in the scene are going to rotate of translate
// or possibly collide against the tool. If the environment
// is entirely static, you can set this parameter to "false"
tool->enableDynamicObjects(true);
// ajust the color of the tool
// tool->m_materialProxy = tool->m_materialProxyButtonPressed;
// read the scale factor between the physical workspace of the haptic
// device and the virtual workspace defined for the tool
double workspaceScaleFactor = tool->getWorkspaceScaleFactor();
// define a maximum stiffness that can be handled by the current
// haptic device. The value is scaled to take into account the
// workspace scale factor
stiffnessMax = info.m_maxLinearStiffness / workspaceScaleFactor;
// create a small white line that will be enabled every time the operator
// grasps an object. The line indicated the connection between the
// position of the tool and the grasp position on the object
graspLine = new cShapeLine(cVector3d(0,0,0), cVector3d(0,0,0));
world->addChild(graspLine);
graspLine->m_colorPointA.set(1.0, 1.0, 1.0);
graspLine->m_colorPointB.set(1.0, 1.0, 1.0);
graspLine->setShowEnabled(false);
//-----------------------------------------------------------------------
// COMPOSE THE VIRTUAL SCENE
//-----------------------------------------------------------------------
// create an ODE world to simulate dynamic bodies
ODEWorld = new cODEWorld(world);
// add ODE world as a node inside world
world->addChild(ODEWorld);
// set some gravity
ODEWorld->setGravity(cVector3d(0.0, 0.0, -9.81));
// create a new ODE object that is automatically added to the ODE world
ODEBody0 = new cODEGenericBody(ODEWorld);
ODEBody1 = new cODEGenericBody(ODEWorld);
ODEBody2 = new cODEGenericBody(ODEWorld);
// create a virtual mesh that will be used for the geometry
// representation of the dynamic body
cMesh* object0 = new cMesh();
cMesh* object1 = new cMesh();
cMesh* object2 = new cMesh();
// crate a cube mesh
double boxSize = 0.2;
createCube(object0, boxSize);
createCube(object1, boxSize);
createCube(object2, boxSize);
// define some material properties for each cube
cMaterial mat0, mat1, mat2;
mat0.m_ambient.set(0.8, 0.1, 0.4);
mat0.m_diffuse.set(1.0, 0.15, 0.5);
mat0.m_specular.set(1.0, 0.2, 0.8);
mat0.setRedIndian();
mat0.m_specular.set(0.0, 0.0, 0.0);
mat0.setStiffness(0.5 * stiffnessMax);
mat0.setDynamicFriction(0.6);
mat0.setStaticFriction(0.6);
object0->setMaterial(mat0);
mat1.m_ambient.set(0.2, 0.6, 0.0);
mat1.m_diffuse.set(0.2, 0.8, 0.0);
mat1.m_specular.set(0.2, 1.0, 0.0);
mat1.setBlueRoyal();
mat1.m_specular.set(0.0, 0.0, 0.0);
mat1.setStiffness(0.5 * stiffnessMax);
mat1.setDynamicFriction(0.6);
mat1.setStaticFriction(0.6);
object1->setMaterial(mat1);
mat2.m_ambient.set(0.0, 0.2, 0.6);
mat2.m_diffuse.set(0.0, 0.2, 0.8);
mat2.m_specular.set(0.0, 0.2, 1.0);
mat2.setGreenDarkSea();
mat2.m_specular.set(0.0, 0.0, 0.0);
mat2.setStiffness(0.5 * stiffnessMax);
mat2.setDynamicFriction(0.6);
mat2.setStaticFriction(0.6);
object2->setMaterial(mat2);
// add mesh to ODE object
ODEBody0->setImageModel(object0);
ODEBody1->setImageModel(object1);
ODEBody2->setImageModel(object2);
// create a dynamic model of the ODE object. Here we decide to use a box just like
// the object mesh we just defined
ODEBody0->createDynamicBox(boxSize, boxSize, boxSize);
ODEBody1->createDynamicBox(boxSize, boxSize, boxSize, false, cVector3d(1,1,1));
ODEBody2->createDynamicBox(boxSize, boxSize, boxSize);
// define some mass properties for each cube
ODEBody0->setMass(0.05);
ODEBody1->setMass(0.05);
ODEBody2->setMass(0.05);
// set position of each cube
cVector3d tmpvct;
tmpvct = cVector3d(0.0,-0.6, -0.5);
ODEBody0->setPosition(tmpvct);
tmpvct = cVector3d(0.0, 0.6, -0.5);
ODEBody1->setPosition(tmpvct);
tmpvct = cVector3d(0.0, 0.0, -0.5);
ODEBody2->setPosition(tmpvct);
// rotate central cube of 45 degrees (just to show hoe this works!)
cMatrix3d rot;
rot.identity();
rot.rotateAboutGlobalAxisRad(cVector3d(0,0,1), cDegToRad(45));
ODEBody0->setRotation(rot);
// we create 6 static walls to contains the 3 cubes within a limited workspace
ODEGPlane0 = new cODEGenericBody(ODEWorld);
ODEGPlane1 = new cODEGenericBody(ODEWorld);
ODEGPlane2 = new cODEGenericBody(ODEWorld);
ODEGPlane3 = new cODEGenericBody(ODEWorld);
ODEGPlane4 = new cODEGenericBody(ODEWorld);
ODEGPlane5 = new cODEGenericBody(ODEWorld);
double size = 1.0;
ODEGPlane0->createStaticPlane(cVector3d(0.0, 0.0, 2.0 *size), cVector3d(0.0, 0.0 ,-1.0));
ODEGPlane1->createStaticPlane(cVector3d(0.0, 0.0, -size), cVector3d(0.0, 0.0 , 1.0));
ODEGPlane2->createStaticPlane(cVector3d(0.0, size, 0.0), cVector3d(0.0,-1.0, 0.0));
ODEGPlane3->createStaticPlane(cVector3d(0.0, -size, 0.0), cVector3d(0.0, 1.0, 0.0));
ODEGPlane4->createStaticPlane(cVector3d( size, 0.0, 0.0), cVector3d(-1.0,0.0, 0.0));
ODEGPlane5->createStaticPlane(cVector3d(-0.8 * size, 0.0, 0.0), cVector3d( 1.0,0.0, 0.0));
//////////////////////////////////////////////////////////////////////////
// Create some reflexion
//////////////////////////////////////////////////////////////////////////
/*
// we create an intermediate node to which we will attach
// a copy of the object located inside the world
cGenericObject* reflexion = new cGenericObject();
// set this object as a ghost node so that no haptic interactions or
// collision detecting take place within the child nodes added to the
// reflexion node.
reflexion->setGhostEnabled(true);
// add reflexion node to world
world->addChild(reflexion);
// turn off culling on each object (objects now get rendered on both sides)
object0->setUseCulling(false, true);
object1->setUseCulling(false, true);
object2->setUseCulling(false, true);
// create a symmetry rotation matrix (z-plane)
cMatrix3d rotRefexion;
rotRefexion.set(1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0);
reflexion->setLocalRot(rotRefexion);
reflexion->setLocalPos(0.0, 0.0, -2.005);
// add objects to the world
reflexion->addChild(ODEWorld);
reflexion->addChild(tool);
*/
//////////////////////////////////////////////////////////////////////////
// Create a Ground
//////////////////////////////////////////////////////////////////////////
// create mesh to model ground surface
cMesh* ground = new cMesh();
world->addChild(ground);
//cCreateCylinder(ground, 0.01, 2.0, 36, 1, true, true);
// create 4 vertices (one at each corner)
double groundSize = 1.3;
int vertices0 = ground->newVertex(-groundSize, -1.0 *groundSize, 0.0);
int vertices1 = ground->newVertex( groundSize, -1.0 *groundSize, 0.0);
int vertices2 = ground->newVertex( groundSize, 1.0 *groundSize, 0.0);
int vertices3 = ground->newVertex(-groundSize, 1.0 *groundSize, 0.0);
// compose surface with 2 triangles
ground->newTriangle(vertices0, vertices1, vertices2);
ground->newTriangle(vertices0, vertices2, vertices3);
// compute surface normals
ground->computeAllNormals();
// position ground at the right level
ground->setLocalPos(0.0, 0.0, -1.0);
// define some material properties and apply to mesh
cMaterial matGround;
matGround.setStiffness(0.5 * stiffnessMax);
matGround.setDynamicFriction(0.2);
matGround.setStaticFriction(0.0);
// matGround.m_ambient.set(0.0, 0.0, 0.0);
// matGround.m_diffuse.set(0.0, 0.0, 0.0);
// matGround.m_specular.set(0.0, 0.0, 0.0);
matGround.setWhite();
matGround.m_emission.setGrayLevel(0.2);
ground->setMaterial(matGround);
// enable and set transparency level of ground
// ground->setTransparencyLevel(0.7);
// ground->setUseTransparency(true);
// setup collision detector
ground->createAABBCollisionDetector(proxyRadius);
//-----------------------------------------------------------------------
// OPEN GL - WINDOW DISPLAY
//-----------------------------------------------------------------------
// initialize GLUT
glutInit(&argc, argv);
// retrieve the resolution of the computer display and estimate the position
// of the GLUT window so that it is located at the center of the screen
int screenW = glutGet(GLUT_SCREEN_WIDTH);
int screenH = glutGet(GLUT_SCREEN_HEIGHT);
int windowPosX = (screenW - WINDOW_SIZE_W) / 2;
int windowPosY = (screenH - WINDOW_SIZE_H) / 2;
// initialize the OpenGL GLUT window
glutInitWindowPosition(windowPosX, windowPosY);
glutInitWindowSize(WINDOW_SIZE_W, WINDOW_SIZE_H);
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
glutCreateWindow(argv[0]);
glutDisplayFunc(updateGraphics);
glutKeyboardFunc(keySelect);
glutReshapeFunc(resizeWindow);
glutSetWindowTitle("CHAI3D");
// create a mouse menu (right button)
glutCreateMenu(menuSelect);
glutAddMenuEntry("full screen", OPTION_FULLSCREEN);
glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);
glutAttachMenu(GLUT_RIGHT_BUTTON);
//-----------------------------------------------------------------------
// START SIMULATION
//-----------------------------------------------------------------------
// simulation in now running
simulationRunning = true;
// create a thread which starts the main haptics rendering loop
cThread* hapticsThread = new cThread();
hapticsThread->start(updateHaptics, CTHREAD_PRIORITY_HAPTICS);
// start the main graphics rendering loop
glutTimerFunc(30, graphicsTimer, 0);
glutMainLoop();
// close everything
close();
// exit
return (0);
}
//===========================================================================
bool cEffectMagnet::computeForce(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int& a_toolID,
cVector3d& a_reactionForce)
{
// compute distance from object to tool
double distance = cDistance(a_toolPos, m_parent->m_interactionProjectedPoint);
// get parameters of magnet
double magnetMaxForce = m_parent->m_material.getMagnetMaxForce();
double magnetMaxDistance = m_parent->m_material.getMagnetMaxDistance();
double stiffness = m_parent->m_material.getStiffness();
double forceMagnitude = 0;
if ((distance > 0) && (distance < magnetMaxDistance) && (stiffness > 0))
{
double limitLinearModel = magnetMaxForce / stiffness;
cClamp(limitLinearModel, 0.0, 0.5 * distance);
if (distance < limitLinearModel)
{
// apply local linear model near magnet
forceMagnitude = stiffness * distance;
}
else
{
// compute quadratic model
cMatrix3d sys;
sys.m[0][0] = limitLinearModel * limitLinearModel;
sys.m[0][1] = limitLinearModel;
sys.m[0][2] = 1.0;
sys.m[1][0] = magnetMaxDistance * magnetMaxDistance;
sys.m[1][1] = magnetMaxDistance;
sys.m[1][2] = 1.0;
sys.m[2][0] = 2.0 * limitLinearModel;
sys.m[2][1] = 1.0;
sys.m[2][2] = 0.0;
sys.invert();
cVector3d param;
sys.mulr(cVector3d(magnetMaxForce, 0.0, -1.0), param);
// apply quadratic model
double val = distance - limitLinearModel;
forceMagnitude = param.x * val * val + param.y * val + param.z;
}
// compute magnetic force
a_reactionForce = cMul(forceMagnitude, cNormalize(cSub(m_parent->m_interactionProjectedPoint, a_toolPos)));
// add damping component
double viscosity = m_parent->m_material.getViscosity();
cVector3d viscousForce = cMul(-viscosity, a_toolVel);
a_reactionForce.add(viscousForce);
return (true);
}
else
{
// the tool is located outside the magnet zone
a_reactionForce.zero();
return (false);
}
}
