void solve(solver stepAlgorithm, double tMax, char *filename, int plots)
{
initialize();
double t = 0.0;
int step = 0;
int plot = 0;
FILE *out;
char filename_tmp[1024];
int j;
double rho_avg = 0.0, u_avg = 0.0, e_avg = 0.0, P_avg = 0.0;
double rho, u, e, P;
tau = CFL * h / cMax();
while(plot<=plots) {
sprintf(filename_tmp, "%s_step_%d.dat", filename, plot);
if(!(out = fopen(filename_tmp, "w"))){
fprintf(stderr, "problem opening file %s\n", filename);
exit(1);
}
// write solution in plot files and print
double rho_avg = 0.0, u_avg = 0.0, e_avg = 0.0, P_avg = 0.0;
for (j = 0; j < N; j++) {
rho = U[j][0];
u = U[j][1] / U[j][0];
e = U[j][2];
P = (U[j][2] - U[j][1] * U[j][1] / U[j][0] / 2)
* (gama - 1.0);
rho_avg += rho;
u_avg += u;
e_avg += e;
P_avg += P;
fprintf(out, "%d\t%f\t%f\t%f\t%f\n", j, rho, u, e, P);
}
fclose(out);
if (rho_avg != 0.0){ rho_avg /= N;}
if (u_avg != 0.0){ u_avg /= N;}
if (e_avg != 0.0){ e_avg /= N;}
if (P_avg != 0.0){ P_avg /= N;}
fprintf(stdout,"Step %d Time %f\tRho_avg %f\t u_avg %f\t e_avg %f\t P_avg %f\n",
step, t,rho_avg,u_avg,e_avg,P_avg);
plot++;
while (t < tMax * plot / (double)(plots)) {
boundaryConditions(U);
tau = CFL * h / cMax();
stepAlgorithm();
t += tau;
step++;
}
}
}
void initialize() {
int j;
double rho,p,u,e;
allocate();
h = 1.0 * L / (N - 1);
for (j = 0; j < N; j++) {
rho = 1;
p = 1;
u = 0;
if (j > N / 2){
rho = 0.125;
p = 0.1;
}
e = p/(gama-1) + rho*u*u/2.0;
U[j][0] = rho;
U[j][1] = rho*u;
U[j][2] = e;
F[j][0] = rho*u;
F[j][1] = rho*u*u+p;
F[j][2] = u*(e+p);
}
tau = CFL*h/cMax();
step = 0;
}
void solve(double tMax)
{
initialize();
double t = 0.0;
double rho, u, e, P;
tau = CFL*h/cMax();
while (t < tMax) {
boundaryConditions(U);
tau = CFL*h/cMax();
upwindGodunovStep();
t += tau;
}
//File to plot last step
FILE *final_step_file;
final_step_file = fopen("final_step.dat", "w");
double current_x;
int j;
for (j = 0; j < N; j++) {
rho = U[j][0];
u = U[j][1]/rho;
e = U[j][2];
P = (gama-1.0)*(e-rho*u*u/2.0);
current_x = j*h;
fprintf(final_step_file, "%f\t%.20f\t%.20f\t%.20f\t%.20f\n", current_x, rho, u, e, P);
}
}
void drawClouds(const Vector& viewer)
{
glShadeModel(GL_SMOOTH);
glDisable(GL_TEXTURE_2D);
glDisable(GL_BLEND);
glDisable(GL_ALPHA_TEST);
Vector cMin(-13500,2000,-13500), cMax(13500,2000,13500);
float dX = 100, dZ = 100;
for(float z=cMin.z; z<cMax.z; z+=dZ){
int j1 = int((z - CloudsMin.z) / (CloudsMax.z-CloudsMin.z) * (CloudsReso-1));
int j2 = int((z+dZ - CloudsMin.z) / (CloudsMax.z-CloudsMin.z) * (CloudsReso-1));
glBegin(GL_QUAD_STRIP);
for(float x=cMin.x; x<cMax.x; x+=dX){
int i = int((x - CloudsMin.x) / (CloudsMax.x-CloudsMin.x) * (CloudsReso-1));
float k1 = cloudsMap[j1 * CloudsReso + i];
float k2 = cloudsMap[j2 * CloudsReso + i];
float s1 = k1 * .2f + .8f;
float s2 = k2 * .2f + .8f;
Vector v1(x, cMin.y+k1*500, z);
Vector v2(x, cMin.y+k2*500, z+dZ);
glColor4f(s1,s1,s1,k1);
glVertex(v1);
glColor4f(s2,s2,s2,k2);
glVertex(v2);
}
glEnd();
glBegin(GL_QUAD_STRIP);
for(float x=cMin.x; x<cMax.x; x+=dX){
int i = int((x - CloudsMin.x) / (CloudsMax.x-CloudsMin.x) * (CloudsReso-1));
float k1 = cloudsMap[j1 * CloudsReso + i];
float k2 = cloudsMap[j2 * CloudsReso + i];
float s1 = k1 * -.2f + .8f;
float s2 = k2 * -.2f + .8f;
Vector v1(x, cMin.y-k1*100, z);
Vector v2(x, cMin.y-k2*100, z+dZ);
glColor4f(s2,s2,s2,k2);
glVertex(v2);
glColor4f(s1,s1,s1,k1);
glVertex(v1);
}
glEnd();
}
}
bool ch_proxyPointForceAlgo::computeNextProxyPositionWithContraints00(const cVector3d& a_goalGlobalPos, const cVector3d& a_toolVel)
{
// We define the goal position of the proxy.
cVector3d goalGlobalPos = a_goalGlobalPos;
// To address numerical errors of the computer, we make sure to keep the proxy
// slightly above any triangle and not directly on it. If we are using a radius of
// zero, we need to define a default small value for epsilon
m_epsilonInitialValue = cAbs(0.0001 * m_radius);
if (m_epsilonInitialValue < m_epsilonBaseValue)
{
m_epsilonInitialValue = m_epsilonBaseValue;
}
// The epsilon value is dynamic (can be reduced). We set it to its initial
// value if the proxy is not touching any triangle.
if (m_numContacts == 0)
{
m_epsilon = m_epsilonInitialValue;
m_slipping = true;
}
// If the distance between the proxy and the goal position (device) is
// very small then we can be considered done.
if (!m_useDynamicProxy)
{
if (goalAchieved(m_proxyGlobalPos, goalGlobalPos))
{
m_nextBestProxyGlobalPos = m_proxyGlobalPos;
m_algoCounter = 0;
return (false);
}
}
// compute the normalized form of the vector going from the
// current proxy position to the desired goal position
// compute the distance between the proxy and the goal positions
double distanceProxyGoal = cDistance(m_proxyGlobalPos, goalGlobalPos);
// A vector from the proxy to the goal
cVector3d vProxyToGoal;
cVector3d vProxyToGoalNormalized;
bool proxyAndDeviceEqual;
if (distanceProxyGoal > m_epsilon)
{
// proxy and goal are sufficiently distant from each other
goalGlobalPos.subr(m_proxyGlobalPos, vProxyToGoal);
vProxyToGoal.normalizer(vProxyToGoalNormalized);
proxyAndDeviceEqual = false;
}
else
{
// proxy and goal are very close to each other
vProxyToGoal.zero();
vProxyToGoalNormalized.zero();
proxyAndDeviceEqual = true;
}
// 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;
if (m_useDynamicProxy)
{
targetPos = goalGlobalPos;
}
else
{
targetPos = goalGlobalPos +
cMul(m_epsilonCollisionDetection, vProxyToGoalNormalized);
}
// setup collision detector
// m_radius is the radius of the proxy
m_collisionSettings.m_collisionRadius = m_radius;
// Search for a collision between the first segment (proxy-device)
// and the environment.
m_collisionSettings.m_adjustObjectMotion = m_useDynamicProxy;
m_collisionRecorderConstraint0.clear();
bool hit = m_world->computeCollisionDetection(m_proxyGlobalPos,
targetPos,
m_collisionRecorderConstraint0,
m_collisionSettings);
// check if collision occurred between proxy and goal positions.
double collisionDistance;
if (hit)
{
collisionDistance = sqrt(m_collisionRecorderConstraint0.m_nearestCollision.m_squareDistance);
if (m_useDynamicProxy)
{
// retrieve new position of proxy
cVector3d posLocal = m_collisionRecorderConstraint0.m_nearestCollision.m_adjustedSegmentAPoint;
cGenericObject* obj = m_collisionRecorderConstraint0.m_nearestCollision.m_object;
cVector3d posGlobal = cAdd(obj->getGlobalPos(), cMul( obj->getGlobalRot(), posLocal ));
m_proxyGlobalPos = posGlobal;
distanceProxyGoal = cDistance(m_proxyGlobalPos, goalGlobalPos);
goalGlobalPos.subr(m_proxyGlobalPos, vProxyToGoal);
vProxyToGoal.normalizer(vProxyToGoalNormalized);
}
if (collisionDistance > (distanceProxyGoal + CHAI_SMALL))
{
// just to make sure that the collision point lies on the proxy-goal segment and not outside of it
hit = false;
}
if (hit)
{
// 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, then we move the proxy to its goal, and we're done
if (!hit)
{
m_numContacts = 0;
m_algoCounter = 0;
m_slipping = true;
m_nextBestProxyGlobalPos = goalGlobalPos;
return (false);
}
// a first collision has occurred
m_algoCounter = 1;
//-----------------------------------------------------------------------
// FIRST 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_collisionRecorderConstraint0.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_contactPoint0->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_contactPoint0->m_globalPos.addr(vColNextGoal, nextProxyPos);
// we can now set the next position of the proxy
m_nextBestProxyGlobalPos = nextProxyPos;
// If the distance between the proxy and the goal position (device) is
// very small then we can be considered done.
if (goalAchieved(goalGlobalPos, nextProxyPos))
{
m_numContacts = 1;
m_algoCounter = 0;
return (true);
}
return (true);
}
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);
}
/***
Enable or disable haptics; called when the user clicks
the enable/disable haptics button. The "enable" parameter
is one of :
#define TOGGLE_HAPTICS_TOGGLE -1
#define TOGGLE_HAPTICS_DISABLE 0
#define TOGGLE_HAPTICS_ENABLE 1
***/
void Cmesh_mesh_collisionsApp::toggle_haptics(int enable) {
if (enable == TOGGLE_HAPTICS_TOGGLE) {
if (haptics_enabled) toggle_haptics(TOGGLE_HAPTICS_DISABLE);
else toggle_haptics(TOGGLE_HAPTICS_ENABLE);
}
else if (enable == TOGGLE_HAPTICS_ENABLE) {
if (haptics_enabled) return;
haptics_enabled = 1;
// create a phantom tool with its graphical representation
//
// Use device zero, and use either the gstEffect or direct
// i/o communication mode, depending on the USE_PHANTOM_DIRECT_IO
// constant
if (tool == 0) {
// Create a new tool with this mesh
tool = new cMeshTool(world, 0, true);
world->addChild(tool);
// Load a gear mesh from a .3DS file
tool_object = new cMesh(world);
tool_object->loadFromFile("resources\\models\\small_gear.3ds");
tool_object->computeGlobalPositions(false);
// Scale the object to fit nicely in our viewport
// compute size of object
tool_object->computeBoundaryBox(true);
cVector3d min = tool_object->getBoundaryMin();
cVector3d max = tool_object->getBoundaryMax();
// This is the "size" of the object
cVector3d span = cSub(max, min);
double size = cMax(span.x, cMax(span.y, span.z));
// We'll center all vertices, then multiply by this amount,
// to scale to the desired size.
double scaleFactor = 2.0 / size;
tool_object->scale(scaleFactor);
// Create a sphere tree bounding volume hierarchy for collision detection on this mesh
tool_object->createSphereTreeCollisionDetector(true, true);
// Use vertex colors so we can see which triangles collide
tool_object->useColors(true, true);
// Add the mesh object to the world
world->addChild(tool_object);
// Set the mesh for this tool
tool->setMesh(tool_object);
// Tell the tool to search for collisions with this mesh
tool->addCollisionMesh(object);
// Set up the device
tool->initialize();
// Set up a nice-looking workspace for the phantom so
// it fits nicely with our shape
tool->setWorkspace(2.0, 2.0, 2.0);
// Rotate the tool so its axes align with our opengl-like axes
tool->rotate(cVector3d(0,0,1), -90.0*3.14159/180.0);
tool->rotate(cVector3d(1,0,0), -90.0*3.14159/180.0);
tool->setRadius(0.05);
}
// I need to call this so the tool can update its internal
// transformations before performing collision detection, etc.
tool->computeGlobalPositions();
// Open communication with the device
tool->start();
// Enable forces
tool->setForcesON();
// Tell the proxy algorithm associated with this tool to enable its
// "dynamic mode", which allows interaction with moving objects
// The dynamic proxy is in a pretty beta state, so we turn it off for now...
// tool->getProxy()->enableDynamicProxy(1);
#ifdef USE_MM_TIMER_FOR_HAPTICS
// start the mm timer to run the haptic loop
timer.set(0,mesh_mesh_collisions_haptic_iteration,this);
#else
// start haptic thread
haptics_thread_running = 1;
DWORD thread_id;
::CreateThread(0, 0, (LPTHREAD_START_ROUTINE)(mesh_mesh_collisions_haptic_loop), this, 0, &thread_id);
// Boost thread and process priority
::SetThreadPriority(&thread_id, THREAD_PRIORITY_ABOVE_NORMAL);
//::SetPriorityClass(GetCurrentProcess(),ABOVE_NORMAL_PRIORITY_CLASS);
#endif
} // enabling
else if (enable == TOGGLE_HAPTICS_DISABLE) {
// Don't do anything if haptics are already off
if (haptics_enabled == 0) return;
// tell the haptic thread to quit
haptics_enabled = 0;
#ifdef USE_MM_TIMER_FOR_HAPTICS
timer.stop();
#else
// wait for the haptic thread to quit
while(haptics_thread_running) Sleep(1);
#endif
// Stop the haptic device...
tool->setForcesOFF();
tool->stop();
// SetPriorityClass(GetCurrentProcess(),NORMAL_PRIORITY_CLASS);
} // disabling
} // toggle_haptics()
BOOL Cmesh_mesh_collisionsApp::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 Cmesh_mesh_collisionsDlg;
m_pMainWnd = g_main_dlg;
g_main_dlg->Create(IDD_mesh_mesh_collisions_DIALOG,NULL);
// Now we should have a display context to work with...
// Create a world and set a white background color
world = new cWorld();
world->setBackgroundColor(1.0,1.0,1.0);
// Create a camera and set its position, look-at point, and orientation (up-direction)
camera = new cCamera(world);
int result = camera->set(cVector3d(0,0,4), cVector3d(0,0,0), cVector3d(0,1,0));
// Create, enable, and position a light source
light = new cLight(world);
light->setEnabled(true);
light->setPos(cVector3d(0,1,4));
// Create a display for graphic rendering
viewport = new cViewport(g_main_dlg->m_gl_area_hwnd, camera, false);
// Load a gear mesh from a .3DS file
object = new cMesh(world);
object->loadFromFile("resources\\models\\small_gear.3ds");
// Scale the object to fit nicely in our viewport
// compute size of object
object->computeBoundaryBox(true);
cVector3d min = object->getBoundaryMin();
cVector3d max = object->getBoundaryMax();
// This is the "size" of the object
cVector3d span = cSub(max, min);
double size = cMax(span.x, cMax(span.y, span.z));
// We'll center all vertices, then multiply by this amount,
// to scale to the desired size.
double scaleFactor = 2.0 / size;
object->scale(scaleFactor);
// Tell him to compute a bounding box...
object->computeBoundaryBox(true);
// Build a nice collision-detector for this object
object->createSphereTreeCollisionDetector(true,true);
// Automatically compute normals for all triangles
object->computeAllNormals();
// Translate and rotate so that the airplane is flying towards the right of the screen
object->translate(0.7, 0.0, 0.0);
object->rotate(cVector3d(0,1,0),-90.0 * 3.14159 / 180.0);
object->rotate(cVector3d(1,0,0),-30.0 * 3.14159 / 180.0);
object->computeGlobalPositions(false);
// Use vertex colors so we can see which triangles collide
object->useColors(true, true);
// Add the mesh object to the world
world->addChild(object);
world->computeGlobalPositions();
m_show_all = 1;
return TRUE;
}
//===========================================================================
void cFog::setProperties(const double a_start, const double a_end, const double a_density)
{
m_start = (float)a_start;
m_end = (float)cMax(a_start, a_end);
m_density = (float)a_density;
}
//==============================================================================
int cCollisionAABB::buildTree(const int a_indexFirstNode, const int a_indexLastNode, const int a_depth)
{
// create new node
cCollisionAABBNode node;
// set depth of this node.
node.m_depth = a_depth;
node.m_nodeType = C_AABB_NODE_INTERNAL;
// create a box to enclose all the leafs below this internal node
node.m_bbox.setEmpty();
for (int i=a_indexFirstNode; i<=a_indexLastNode; i++)
{
node.m_bbox.enclose(m_nodes[i].m_bbox);
}
// move leafs with smaller coordinates (on the longest axis) towards the
// beginning of the array and leaves with larger coordinates towards the
// end of the array
int axis = node.m_bbox.getLongestAxis();
int i = a_indexFirstNode;
int mid = a_indexLastNode;
double center = node.m_bbox.getCenter().get(axis);
while (i < mid)
{
if (m_nodes[i].m_bbox.getCenter().get(axis) < center)
{
i++;
}
else
{
// swap nodes. For efficiency, we swap the minimum amount of information necessary.
int t_leftSubTree = m_nodes[i].m_leftSubTree;
cCollisionAABBBox t_bbox = m_nodes[i].m_bbox;
m_nodes[i].m_leftSubTree = m_nodes[mid].m_leftSubTree;
m_nodes[i].m_bbox = m_nodes[mid].m_bbox;
m_nodes[mid].m_leftSubTree = t_leftSubTree;
m_nodes[mid].m_bbox = t_bbox;
//cSwap(m_nodes[i], m_nodes[mid]);
mid--;
}
}
// increment depth for child nodes
int depth = a_depth + 1;
m_maxDepth = cMax(m_maxDepth, depth);
// we expect mid, used as the right iterator in the "insertion sort" style
// rearrangement above, to have moved roughly to the middle of the array;
// however, if it never moved left or moved all the way left, set it to
// the middle of the array so that neither the left nor right subtree will
// be empty
if ((mid == a_indexFirstNode) || (mid == a_indexLastNode))
{
mid = (a_indexLastNode + a_indexFirstNode) / 2;
}
// if there are only two nodes then assign both child nodes as leaves
if ((a_indexLastNode - a_indexFirstNode) == 1)
{
// set left leaf
node.m_leftSubTree = a_indexFirstNode;
m_nodes[a_indexFirstNode].m_depth = depth;
// set right leaf
node.m_rightSubTree = a_indexLastNode;
m_nodes[a_indexLastNode].m_depth = depth;
}
// there are more than 2 nodes
else
{
// if the left subtree contains multiple elements, create new internal node
if (mid > a_indexFirstNode)
{
node.m_leftSubTree = buildTree(a_indexFirstNode, mid, depth);
}
// if there is only one element in the right subtree, the right subtree
// pointer should just point to the leaf node
else
{
node.m_leftSubTree = a_indexFirstNode;
m_nodes[a_indexFirstNode].m_depth = depth;
}
// if the right subtree contains multiple elements, create new internal node
if ((mid+1) < a_indexLastNode)
{
node.m_rightSubTree = buildTree((mid+1), a_indexLastNode, depth);
}
// if there is only one element in the left subtree, the left subtree
// pointer should just point to the leaf node
else
{
node.m_rightSubTree = a_indexLastNode;
m_nodes[a_indexLastNode].m_depth = depth;
}
}
// insert node
m_nodes.push_back(node);
return (m_nodes.size()-1);
}
int loadHeightMap()
{
// create a texture file
cTexture2D* newTexture = new cTexture2D();
world->addTexture(newTexture);
// texture 2D
bool fileload = newTexture->loadFromFile(RESOURCE_PATH("resources/images/map.bmp"));
if (!fileload)
{
#if defined(_MSVC)
fileload = newTexture->loadFromFile("../../../bin/resources/images/map.bmp");
#endif
}
if (!fileload)
{
printf("Error - Texture image failed to load correctly.\n");
close();
return (-1);
}
// get the size of the texture image (U and V)
int texSizeU = newTexture->m_image.getWidth();
int texSizeV = newTexture->m_image.getHeight();
// check size of image
if ((texSizeU < 1) || (texSizeV < 1)) { return (false); }
// we look for the largest side
int largestSide;
if (texSizeU > texSizeV)
{
largestSide = texSizeU;
}
else
{
largestSide = texSizeV;
}
// The largest side of the map has a length of 1.0
// we now compute the respective size for 1 pixel of the image in world space.
double size = 1.0 / (double)largestSide;
// we will create an triangle based object. For centering puposes we
// compute an offset for axis X and Y corresponding to the half size
// of the image map.
double offsetU = 0.5 * (double)texSizeU * size;
double offsetV = 0.5 * (double)texSizeV * size;
// For each pixel of the image, create a vertex
int u,v;
for (v=0; v<texSizeV; v++)
{
for (u=0; u<texSizeU; u++)
{
double px, py, tu, tv;
// compute the height of the vertex
cColorb color = newTexture->m_image.getPixelColor(u,v);
double height = 0.1 * ((double)color.getR() + (double)color.getG() + (double)color.getB()) / (3.0 * 255.0);
// compute the position of the vertex
px = size * (double)u - offsetU;
py = size * (double)v - offsetV;
// create new vertex
unsigned int index = object->newVertex(px, py, height);
cVertex* vertex = object->getVertex(index);
// compute texture coordinate
tu = (double)u / texSizeU;
tv = (double)v / texSizeV;
vertex->setTexCoord(tu, tv);
}
}
// Create a triangle based map using the above pixels
for (v=0; v<(texSizeV-1); v++)
{
for (u=0; u<(texSizeU-1); u++)
{
// get the indexing numbers of the next four vertices
unsigned int index00 = ((v + 0) * texSizeU) + (u + 0);
unsigned int index01 = ((v + 0) * texSizeU) + (u + 1);
unsigned int index10 = ((v + 1) * texSizeU) + (u + 0);
unsigned int index11 = ((v + 1) * texSizeU) + (u + 1);
// create two new triangles
object->newTriangle(index00, index01, index10);
object->newTriangle(index10, index01, index11);
}
}
// apply texture to object
object->setTexture(newTexture);
object->setUseTexture(true);
// compute normals
object->computeAllNormals(true);
// compute size of object
object->computeBoundaryBox(true);
cVector3d min = object->getBoundaryMin();
cVector3d max = object->getBoundaryMax();
// This is the "size" of the object
cVector3d span = cSub(max, min);
size = cMax(span.x, cMax(span.y, span.z));
// We'll center all vertices, then multiply by this amount,
// to scale to the desired size.
double scaleFactor = MESH_SCALE_SIZE / size;
object->scale(scaleFactor);
// compute size of object again
object->computeBoundaryBox(true);
// Build a collision-detector for this object, so
// the proxy will work nicely when haptics are enabled.
object->createAABBCollisionDetector(1.01 * proxyRadius, true, false);
// set size of frame
object->setFrameSize(0.2, true);
// set size of normals
object->setNormalsProperties(0.01, cColorf(1.0, 0.0, 0.0, 1.0), true);
// render graphically both sides of triangles
object->setUseCulling(false);
// update global position
object->computeGlobalPositions();
// success
return (0);
}
int main(void)
{
double h; // lattice spacing
double tau; // time step
printf("empece\n");
double tMax=1.0;
char filename[80];
sprintf(filename,"UpwindGodunov");
double U[N][3];
initialize(U);
double gamma=1.4;
double t = 0.0;
int step = 0;
int plot = 0;
int i;
FILE *out;
char filename_tmp[1024];
int j;
double rho_avg = 0.0, u_avg = 0.0, e_avg = 0.0, P_avg = 0.0;
double rho, u, e, P;
h = 1.0 * L / (N - 1);
tau = CFL * h / cMax(U);
sprintf(filename_tmp, "%s_step_%d.dat", filename, plot);
if(!(out = fopen(filename_tmp, "w"))){
fprintf(stderr, "problem opening file %s\n", filename);
exit(1);
}
// write solution in plot files and print
for (j = 0; j < N; j++) {
rho = U[j][0];
u = U[j][1] / U[j][0];
e = U[j][2];
P = (U[j][2] - U[j][1] * U[j][1] / U[j][0] / 2)* (gama - 1.0);
rho_avg += rho;
u_avg += u;
e_avg += e;
P_avg += P;
fprintf(out, "%d\t%f\t%f\t%f\t%f\n", j, rho, u, e, P);
}
fclose(out);
double usol[N][3];
obtenerUsol(usol,U);
plot=1;
while (t < tMax+0.1) {
tau = CFL * h / cMax(U);
double htot[N];
for(i=0;i<N;i++){
htot[i]=(gamma/(gamma-1))*(usol[i][2]/usol[i][0])+0.5*pow(usol[i][1],2);
}
double Phi[N-1][3];
for(j=0;j<N-1;j++){
double r=sqrt(usol[j+1][0]/usol[j][0]);
double rm=r*usol[j][0];
double um=(r*usol[j+1][1]+usol[j][1])/(r+1);
double hm=(r*htot[j+1]+htot[j])/(r+1);
double am=sqrt((gamma-1)*(hm-0.5*um*um));
double alfa1=(gamma-1)*um*um/(2*am*am);
double alfa2=(gamma-1)/(am*am);
double Udif[3];
for(i=0;i<3;i++){
Udif[i]=U[j+1][i]-U[j][i];
}
double Pinv[3][3];
Pinv[0][0]=0.5*(alfa1+um/am);
Pinv[0][1]=-0.5*(alfa2*um+1/am);
Pinv[0][2]=alfa2/2;
Pinv[1][0]=1-alfa1;
Pinv[1][1]=alfa2*um;
Pinv[1][2]=-alfa2;
Pinv[2][0]=0.5*(alfa1-um/am);
Pinv[2][1]=-0.5*(alfa2*um-1/am);
Pinv[2][2]=alfa2/2;
double P[3][3];
P[0][0]=1.0;
P[0][1]=1.0;
P[0][2]=1.0;
P[1][0]=um-am;
P[1][1]=um;
P[1][2]=um+am;
P[2][0]=hm-am*um;
P[2][1]=0.5*um*am;
P[2][2]=hm+am*um;
double L[3][3];
L[0][0]=fabs(um-am);
L[0][1]=0;
L[0][2]=0;
L[1][0]=0;
L[1][1]=fabs(um);
L[1][2]=0;
L[2][0]=0;
L[2][1]=0;
L[2][2]=fabs(um+am);
double R[3][3];
multMatrices(P,L,R);
double R1[3][3];
multMatrices(R,Pinv,R1);
double Phi1[3];
matrizVector(R1,Udif,Phi1);
for(i=0;i<3;i++){
Phi[j][i]=Phi1[i];
}
}
double F[N][3];
for(j=0;j<N;j++){
F[j][0]=U[j][1];
double uloc1=U[j][1]/U[j][0];
double rhou2=U[j][1]*uloc1;
double press1=(gamma-1)*(U[j][2]-0.5*rhou2);
F[j][1]=rhou2+press1;
F[j][2]=(U[j][2]+press1)*uloc1;
}
for(j=0;j<N-1;j++){
for(i=0;i<3;i++){
Phi[j][i]*=-0.5;
Phi[j][i]+=0.5*(F[j][i]+F[j+1][i]);
}
}
for(j=1;j<N-1;j++){
for(i=0;i<3;i++){
U[j][i]=U[j][i]-tau/h*(Phi[j][i]-Phi[j-1][i]);
//printf("%f ",w[i][j]);
}
}
if(t>=plot*0.2){
sprintf(filename_tmp, "%s_step_%d.dat", filename, plot);
out = fopen(filename_tmp, "w");
plot++;
for(i=0;i<N;i++){
usol[i][0]=U[i][0];
//printf("%f ",usol[0][i]);
usol[i][1]=U[i][1]/U[i][0];
//printf("%f ",usol[1][i]);
usol[i][2]=(gamma-1)*(U[i][2]-0.5*U[i][1]*usol[i][1]);
//printf("%f \n",usol[2][i]);
fprintf(out, "%d\t%f\t%f\t%f\t%f\n", i, usol[i][0], usol[i][1], U[i][2], usol[i][2]);
}
}
t += tau;
obtenerUsol(usol,U);
}
}
Carro::Carro(dynamicWorld* world):dynamicObject(world,0.5*LARGURACARRO*COMPRIMENTOCARRO,true) {
steeringAngle = 0;
//Instancia o chassi e o meio
meio = new cGenericObject();
chassi = new cGenericObject();
meio->addChild(chassi);
//Adiciona o meio ao carro propriamente dito
this->addChild(meio);
//Inicializa a roda1
roda1 = new cMesh(world);
roda1->loadFromFile("roda_simples.obj");
roda1->computeBoundaryBox(true);
cVector3d min = roda1->getBoundaryMin();
cVector3d max = roda1->getBoundaryMax();
cVector3d meio = cMul(-0.5,cAdd(min,max));
cVector3d span = cSub(max, min);
for(int i=0;i<roda1->getNumVertices(true);i++)
roda1->getVertex(i,true)->translate(meio);
double size = cMax(span.x, cMax(span.y, span.z));
double scaleFactor = 2*RAIORODA / size;
roda1->scale(scaleFactor);
chassi->addChild(roda1);
roda1->translate(0,RAIORODA,LARGURACARRO/2.0);
roda1->rotate(cVector3d(1,0,0),3.1415/2.0);
//Instancia e inicializa a roda2
roda2 = new cMesh(world);
roda2->loadFromFile("roda_simples.obj");
for(int i=0;i<roda2->getNumVertices(true);i++)
roda2->getVertex(i,true)->translate(meio);
roda2->scale(scaleFactor);
chassi->addChild(roda2);
roda2->translate(0,RAIORODA,-LARGURACARRO/2.0);
roda2->rotate(cVector3d(1,0,0),-3.1415/2.0);
//Instanci os eixos do carro
eixo1 = new cGenericObject();
chassi->addChild(eixo1);
eixo2 = new cGenericObject();
chassi->addChild(eixo2);
//Instancia e inicializa a roda3
roda3 = new cMesh(world);
roda3->loadFromFile("roda_simples.obj");
for(int i=0;i<roda3->getNumVertices(true);i++)
roda3->getVertex(i,true)->translate(meio);
roda3->scale(scaleFactor);
eixo1->addChild(roda3);
eixo1->translate(DISTANCIAEIXOS,RAIORODA,LARGURACARRO/2.0);
roda3->rotate(cVector3d(1,0,0),3.1415/2.0);
//Instancia e inicializa a roda4
roda4 = new cMesh(world);
roda4->loadFromFile("roda_simples.obj");
for(int i=0;i<roda4->getNumVertices(true);i++)
roda4->getVertex(i,true)->translate(meio);
roda4->scale(scaleFactor);
eixo2->addChild(roda4);
eixo2->translate(DISTANCIAEIXOS,RAIORODA,-LARGURACARRO/2.0);
roda4->rotate(cVector3d(1,0,0),-3.1415/2.0);
//Instancia e inicializa a carroceria
carroceria = new cMesh(world);
carroceria->loadFromFile("ferrari.3ds");
chassi->addChild(carroceria);
carroceria->computeBoundaryBox(true);
min = carroceria->getBoundaryMin();
max = carroceria->getBoundaryMax();
span = cSub(max, min);
meio = cMul(-0.5,cAdd(min,max));
for(int i=0;i<carroceria->getNumVertices(true);i++)
carroceria->getVertex(i,true)->translate(meio);
size = cMax(span.x, cMax(span.y, span.z));
scaleFactor = COMPRIMENTOCARRO / size;
carroceria->scale(scaleFactor);
carroceria->rotate(cVector3d(0,1,0),3.1415/2.0);
carroceria->rotate(cVector3d(0,0,1),3.1415/2.0);
//recalcula dimensões
carroceria->computeBoundaryBox(true);
min = carroceria->getBoundaryMin();
max = carroceria->getBoundaryMax();
span = cSub(max, min);
double altura= cMin(span.x, cMin(span.y, span.z));
// posiciona carroceria: assumindo que altura do chão é 0.3* raio da roda
// levanta meia altura para chao ficar no plano x,z
carroceria->translate(DISTANCIAEIXOS/1.85,altura*0.5+RAIORODA*0.3,0);
carroceria->useColors(true, true);
carroceria->useMaterial(false,true);
}
//===========================================================================
void cSpotLight::setShadowMapProperties(const double& a_nearClippingPlane,
const double& a_farClippingPlane)
{
m_shadowNearClippingPlane = cMin(cAbs(a_nearClippingPlane), cAbs(a_farClippingPlane));
m_shadowFarClippingPlane = cMax(cAbs(a_nearClippingPlane), cAbs(a_farClippingPlane));
}