//! "sju" Physics Engine for MotorJ /*! * * sju Physics Engine, Physics system for MotorJ * * Handles collision tests and responses between several kinds of geometries, * as well as attraction/repulsion forces, friction fields, etc. * */ /* Copyright (C) 2008-2010 Alejandro Valenzuela Roca, * * This file is part of MotorJ, a free framework for videogame creation. * * * * MotorJ is free software: you can redistribute it and/or modify * it under the terms of the GNU Lesser General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * MotorJ is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU Lesser General Public License for more details. * * You should have received a copy of the GNU Lesser General Public License * along with MotorJ. If not, see . */ #include "sjuengine-class.h" mjPhysics::mjPhysics() { //list_init(&forces); //list_init(&objects); //list_init(&meshes); //list_init(&temp_effects); dragFactor = coulombConstant = gravitationConstant = 0.0; coulombFRadius = 999999999; v3_0(&gravity); coulombModel = CM_NORMAL; SetRayTrDir(NULL); #ifdef PC_SDL update_children = new SDL_Thread* [MAX_PROCESSORS]; update_blocks = new mjUpdateBlock[MAX_PROCESSORS]; #endif } void mjPhysics::SetGravity(vector3 & g) { vec_copy(g, &gravity); } void mjPhysics::SetGravity(float y) { gravity.x = 0.0; gravity.y = y; gravity.z = 0.0; } void mjPhysics::SetGravity(float x, float y, float z) { gravity.x = x; gravity.y = y; gravity.z = z; } void mjPhysics::SetRayTrDir(vector3* dir) { rayTrDir = dir; } void mjPhysics::SetGravitationConstant(float G) { gravitationConstant = G; } void mjPhysics::SetCoulombConstant(float ke) { coulombConstant = ke; } void mjPhysics::SetCoulombFRadius(float radius) { coulombFRadius = radius*radius; } void mjPhysics::AddObject(IPhysicalObject * object) { pMesh * cMesh; std::list::iterator cMeshNode; object->dragFactor = dragFactor; objects.push_back(object); if (object->cGeometry == CG_MESH) { meshes.push_back((pMesh*) object); } // ResetAccels object->ResetAccels(); object->collStatus = COLR_NOCOLLISION; cMeshNode = meshes.begin(); //list_append(0, object, &objects); // while (cMeshNode != meshes.end()) { cMesh = *cMeshNode; //(pMesh *) cMeshNode->data; cMesh->OnNewObjectAdded(objects); cMeshNode++;// = cMeshNode->nxt; } } void mjPhysics::ApplyEffect(IPhysicalObject * object, vector3 & effect, vector3 & pointOfAction, sjuPETypes effectType, IPhysicalObject* interactionObject) { pEffect * newEffect; newEffect = new pEffect; newEffect->obj = object; vec_copy(effect, &newEffect->effect); vec_copy(pointOfAction, &newEffect->pointOfAction); newEffect->effectType = effectType; newEffect->interactionObject = interactionObject; temp_effects.push_back(newEffect); //list_append(0, newEffect, &temp_effects); } void mjPhysics::RemoveEffectsForObject(IPhysicalObject* object) { std::list::iterator cENode; cENode = temp_effects.begin(); while (cENode != temp_effects.end()) { pEffect * cEffect = *cENode; if (cEffect->obj == object) { delete cEffect; cENode = temp_effects.erase(cENode); } else cENode++; } } void mjPhysics::Update(float t_elapsed) { std::list::iterator cENode; std::list::iterator cONode; std::list::iterator cOSecNode; //list_node * cONode; //list_node * cOSecNode; IPhysicalObject * PO_ptr; IPhysicalObject * POSec_ptr; unsigned node1index = 0; unsigned node2index; // To make this really efficient, some kind of scenegraph-like structure is needed here // For now just basic stuff will do.. //cONode = objects.first; //cONode = objects.first; cONode = objects.begin(); while (cONode != objects.end()) { PO_ptr = *cONode; PO_ptr->ResetAccels(); cONode++; } cONode = objects.begin(); while (cONode != objects.end()) { //cENode = temp_effects.first; //PO_ptr = (IPhysicalObject *) cONode->data; PO_ptr = *cONode; // Save current position if different from previous vector3 pos_comparison; vec_copy(PO_ptr->p, &pos_comparison); vec_sum(-1, PO_ptr->oldPos, &pos_comparison); if (vec_norm(pos_comparison)>0.0) vec_copy(PO_ptr->p, &PO_ptr->oldPos); // Apply gravity if (PO_ptr->hasGravity) { PO_ptr->ApplyEffect(gravity, PO_ptr->p, PE_ACCEL, NULL); } cENode = temp_effects.begin(); while (cENode != temp_effects.end()) { pEffect * cEffect = *cENode; //cEffect = (pEffect *) cENode->data; if (PO_ptr == cEffect->obj) { //list_node * dENode; PO_ptr->ApplyEffect(cEffect->effect, cEffect->pointOfAction, cEffect->effectType, cEffect->interactionObject); delete cEffect; //dENode = cENode; //cENode = cENode->nxt; cENode = temp_effects.erase(cENode); //list_yank_node(dENode, &temp_effects); //delete dENode; } else cENode++;// = cENode->nxt; } // Now, this is really a BAD way to do it // this needs optimization // Detect collisions with other objects node2index = node1index + 1; //cOSecNode = cONode->nxt; cOSecNode = cONode; cOSecNode++; while (cOSecNode != objects.end()) { bool collision = false; IPhysicalObject * Arg1 = NULL; IPhysicalObject * Arg2 = NULL; unsigned arg1index; unsigned arg2index; //POSec_ptr = (IPhysicalObject *) cOSecNode->data; POSec_ptr = *cOSecNode; // Apply gravitation if (PO_ptr->hasGravitation && POSec_ptr->hasGravitation) { vector3 primNode_secNode; float d; vec_copy(PO_ptr->p, &primNode_secNode); vec_sum(-1, POSec_ptr->p, &primNode_secNode); d = normalize(&primNode_secNode); d *= d; // d^2 if (d > 0.001) { float k = gravitationConstant/d; mul_scal(k*PO_ptr->mass, &primNode_secNode); POSec_ptr->ApplyEffect(primNode_secNode, POSec_ptr->p, PE_ACCEL, PO_ptr); mul_scal(-1*POSec_ptr->mass/PO_ptr->mass, &primNode_secNode); PO_ptr->ApplyEffect(primNode_secNode, PO_ptr->p, PE_ACCEL, POSec_ptr); } } // Apply Coulomb's law if (PO_ptr->hasCharge && POSec_ptr->hasCharge) { vector3 primNode_secNode; float d; vec_copy(PO_ptr->p, &primNode_secNode); vec_sum(-1, POSec_ptr->p, &primNode_secNode); d = normalize(&primNode_secNode); d *= d; // d^2 if (d < 0.001) d = 0.001; if (d < coulombFRadius) { float k; switch (coulombModel) { case CM_NORMAL: { k = -coulombConstant/d; } break; case CM_CONSTANT: { k = -1.0; } break; } mul_scal(k*PO_ptr->q, &primNode_secNode); POSec_ptr->ApplyEffect(primNode_secNode, POSec_ptr->p, PE_ACCEL, PO_ptr); mul_scal(-1.0*POSec_ptr->q/PO_ptr->q, &primNode_secNode); PO_ptr->ApplyEffect(primNode_secNode, PO_ptr->p, PE_ACCEL, POSec_ptr); } } if ((PO_ptr->cGeometry > POSec_ptr->cGeometry)) { Arg1 = POSec_ptr; arg1index = node2index; Arg2 = PO_ptr; arg2index = node1index; } else { Arg1 = PO_ptr; arg1index = node1index; Arg2 = POSec_ptr; arg2index = node2index; } //printf("Test %x against %x\n", Arg1, Arg2); if (Arg1->hasCollision && Arg2->hasCollision) switch(Arg1->cGeometry) { /*case CG_MESH: { triangle ** triangles = (triangle ** ) Arg1->cGeometryData; pMesh * tm0 = (pMesh*) Arg1; switch(Arg2->cGeometry) { case CG_PT: { unsigned ctriangle = 0; unsigned currCollStatus = COLR_NOCOLLISION; vector3 correction; vector3 minusG; vec_copy(gravity, &minusG); //minusG.y *= -1; while (triangles[ctriangle]) { unsigned prevCollStatus = tm0->collStatuses[ctriangle][arg2index]; currCollStatus = col_ray_tr(Arg2->p, &gravity,// triangles[ctriangle]->n, *triangles[ctriangle], &correction); //if (currCollStatus || prevCollStatus) //printf("Collstatus[%d]: now %d vs %d (%3.2f,%3.2f,%3.2f)\n", //ctriangle, currCollStatus, prevCollStatus, Arg2->p.x, //Arg2->p.y, Arg2->p.z); if (Arg1->objectInteractionEffect == PE_C_IMMOVABLE) { if ((currCollStatus == COLR_UNDER) && ((prevCollStatus == COLR_EXACT) || (prevCollStatus == COLR_OVER))) { //if (Arg1->objectInteractionEffect == PE_C_IMMOVABLE) Arg2->ApplyEffect(*triangles[ctriangle]->n, correction, Arg1->objectInteractionEffect, Arg1); //else //vec_copy(correction, &Arg2->p); currCollStatus = COLR_OVER; collision = true; // Activate the OnCollisionDetected event in the end } } else { if (currCollStatus == COLR_UNDER) { Arg2->ApplyEffect(Arg1->objectInteractionParam, correction, Arg1->objectInteractionEffect, Arg1); collision = true; } } tm0->collStatuses[ctriangle][arg2index] = currCollStatus; ctriangle++; } } break; case CG_SPHERE: { unsigned ctriangle = 0; unsigned currCollStatus; vector3 correction; float radius = * (float *) Arg2->cGeometryData; while (triangles[ctriangle]) { unsigned prevCollStatus = tm0->collStatuses[ctriangle][arg2index]; currCollStatus = col_ray_tr(Arg2->p, triangles[ctriangle]->n, *triangles[ctriangle], &correction); //if (currCollStatus || prevCollStatus) //printf("Collstatus[%d]: now %d vs %d (%3.2f,%3.2f,%3.2f)\n", //ctriangle, currCollStatus, prevCollStatus, Arg2->p.x, //Arg2->p.y, Arg2->p.z); if ((currCollStatus && (dist_p(correction, Arg2->p) < radius)) || ((currCollStatus == COLR_UNDER) && ((prevCollStatus == COLR_EXACT) || (prevCollStatus == COLR_OVER)))) { if (Arg1->objectInteractionEffect == PE_C_IMMOVABLE) Arg2->ApplyEffect(*triangles[ctriangle]->n, correction, Arg1->objectInteractionEffect, Arg1); else Arg2->ApplyEffect(Arg1->objectInteractionParam, correction, Arg1->objectInteractionEffect, Arg1); //vec_copy(correction, &Arg2->p); collision = true; // Activate the OnCollisionDetected event in the end } tm0->collStatuses[ctriangle][arg2index] = currCollStatus; ctriangle++; } } break; default: printf("Case not handled.. mesh vs %d\n", Arg2->cGeometry); break; } } break;*/ case CG_AABB: { // AABB data must be stored as an array of vector3 // As in vector3 myAABB[2] vector3 * aabb0 = (vector3 *) Arg1->cGeometryData; pAABB* Arg1aabb = (pAABB*) Arg1; switch(Arg2->cGeometry) { case CG_PT: { if (col_aabb_pt(aabb0[0], aabb0[1], Arg2->p) == COLR_OVERLAP) { if (Arg1aabb->objectInteractionEffect == PE_C_IMMOVABLE) { //bool not_caught; vector3 normal; vector3 adjust; vector3 correction; vector3 overlap; float dist_corr; v3_1(&normal); v3_0(&adjust); vec_copy(Arg2->p, &overlap); vec_sum(-1, Arg1aabb->p, &overlap); // P - AABBcenter // Get absolute value; catch orientation if (overlap.x < 0.0) { normal.x *= -1.0; overlap.x *= -1.0; } if (overlap.y < 0.0) { normal.y *= -1.0; overlap.y *= -1.0; } if (overlap.z < 0.0) { normal.z *= -1.0; overlap.z *= -1.0; } // Subtract halfWidths vec_sum(-1.0, Arg1aabb->halfWidths, &overlap); // Copy original values to correction vec_copy(Arg2->p, &correction); // Check which is value is closest to 0 (all should be negative) if ((overlap.x > overlap.y) && (overlap.x > overlap.z)) { normal.y = 0; normal.z = 0; // Correct x value correction.x = Arg1aabb->p.x + (sign(normal.x)*Arg1aabb->halfWidths.x); } else if ((overlap.y > overlap.z) && (overlap.y > overlap.z)) { normal.x = 0; normal.z = 0; // Correct y value correction.y = Arg1aabb->p.y + (sign(normal.y)*Arg1aabb->halfWidths.y); } else if ((overlap.z > overlap.x ) && (overlap.z > overlap.y)) { normal.x = 0; normal.y = 0; // Correct z value correction.z = Arg1aabb->p.z + (sign(normal.z)*Arg1aabb->halfWidths.z); } Arg2->ApplyEffect(normal, correction, Arg1->objectInteractionEffect, Arg1); Arg2->collStatus = COLR_EXACT; } else { //printf("Box vs pt effect\n"); Arg2->ApplyEffect(Arg1->objectInteractionParam, Arg2->p, Arg1->objectInteractionEffect, Arg1); } /*if (Arg2->objectInteractionEffect == PE_C_IMMOVABLE) Arg1->ApplyEffect(//FIXME: how to find the appropriate normal?, Arg1->p, Arg1->objectInteractionEffect); else*/ Arg1->ApplyEffect(Arg1->objectInteractionParam, Arg1->p, Arg1->objectInteractionEffect, Arg2); collision = true; // Activate the OnCollisionDetected event in the end } } break; case CG_AABB: { pAABB* Arg2aabb = (pAABB*) Arg2; // if (col_aabb_aabb(Arg1aabb->corners[0], Arg1aabb->corners[1], Arg2aabb->corners[0], Arg2aabb->corners[1])) { collision = true; vector3 traj; vector3 normal; vector3 adjust; vector3 correction; float dist_corr; pAABB* movable; pAABB* fixed; pAABB* movable2 = NULL; bool bothMovable = false; pAABB boxPrevState; if ((Arg1aabb->movable != Arg2aabb->movable) || Arg1aabb->movable) { // Find the movable box if (Arg1aabb->movable) { movable = Arg1aabb; fixed = Arg2aabb; } else { movable = Arg2aabb; fixed = Arg1aabb; } // Are both movable? if (Arg1aabb->movable && Arg2aabb->movable) { movable = Arg1aabb; movable2 = Arg2aabb; fixed = movable2; bothMovable = true; } // Find the closest way to a non-collision spot // Get directions vector3 directions; vec_copy(movable->p, &directions); vec_sum(-1, fixed->p, &directions); // mov_cen - fix_cen vector3 endp[2]; // Find the correct coordinates - the minimum of the "farthest" corners // and the maximum of the "nearest" corners // i.e. the closest set of corners endp[1].x = min(fixed->corners[1].x, movable->corners[1].x); endp[1].y = min(fixed->corners[1].y, movable->corners[1].y); endp[1].z = min(fixed->corners[1].z, movable->corners[1].z); endp[0].x = max(fixed->corners[0].x, movable->corners[0].x); endp[0].y = max(fixed->corners[0].y, movable->corners[0].y); endp[0].z = max(fixed->corners[0].z, movable->corners[0].z); vector3 overlaps; // Compute overlap (distance between closest corners) vec_copy(endp[1], &overlaps); vec_sum(-1, endp[0], &overlaps); vector3 normal; v3_0(&normal); vector3 correction; vector3 correction2; vec_copy(movable->p, &correction); float movAdjust = 1.0; if (bothMovable) { vec_copy(movable2->p, &correction2); } // The direction in which there is the least overlap is the one // we'll try to correct. if ((overlaps.x <= overlaps.y) && (overlaps.x <= overlaps.z)) { // Move in X normal.x = sign(directions.x); correction.x = fixed->p.x + (normal.x*(fixed->halfWidths.x + movable->halfWidths.x)); correction2.x = correction.x - (normal.x*(movable->halfWidths.x + fixed->halfWidths.x)); } else if ((overlaps.y <= overlaps.x) && (overlaps.y <= overlaps.z)) { // Move in y normal.y = sign(directions.y); correction.y = fixed->p.y + (normal.y*(fixed->halfWidths.y + movable->halfWidths.y)); correction2.y = correction.y - (normal.y*(movable->halfWidths.y + fixed->halfWidths.y)); } else { // Move in Z normal.z = sign(directions.z); correction.z = fixed->p.z + (normal.z*(fixed->halfWidths.z + movable->halfWidths.z)); correction2.z = correction.z - (normal.z*(movable->halfWidths.z + fixed->halfWidths.z)); } ApplyEffect(movable, normal, correction, PE_C_IMMOVABLE, fixed); mul_scal(-1,&normal); if (bothMovable) ApplyEffect(fixed, normal, correction2, PE_C_IMMOVABLE, movable); } } } break; } } break; case CG_PT: { switch(Arg2->cGeometry) { //case CG_PT: case CG_SPHERE: { } break; default: break; } } break; case CG_SPHERE: { float radius0 = * (float *) Arg1->cGeometryData; switch(Arg2->cGeometry) { //case CG_PT: case CG_SPHERE: { // Sphere on sphere collision float radius1 = * (float *) Arg2->cGeometryData; vector3 c0c1; if (col_sph_sph(Arg1->p, radius0, Arg2->p, radius1, &c0c1)) { vector3 v1f; vector3 v2f; vector3 correction; float e = (Arg1->e * Arg2->e); // Calculate the restitution coefficient for Arg1Arg2 interaction float invmsum = 1.0/(Arg1->mass + Arg2->mass); float mdiff_e = Arg1->mass - e*Arg2->mass; // Calculate speed resultant for first object vec_copy(Arg1->v, &v1f); // v1 = u1 mul_scal(mdiff_e, &v1f); // v1 = v1*(m1-e*m2) vec_sum((1.0+e)*Arg2->mass, Arg2->v, &v1f); // v1 = v1 + (1+e)m2*u2 mul_scal(invmsum, &v1f); // v1 = v1 / (m1+m2) mdiff_e = Arg2->mass - e*Arg1->mass; // Calculate speed resultant for second object vec_copy(Arg2->v, &v2f); // v2 = u2 mul_scal(mdiff_e, &v2f); // v2 = v2*(m2-e*m1) vec_sum((1.0+e)*Arg1->mass, Arg1->v, &v2f); // v2 = v2 + (1+e)m1*u1 mul_scal(invmsum, &v2f); // v2 = v2 / (m1+m2) // Calculate the correction for the particles normalize(&c0c1); vec_copy(Arg1->p, &correction); float halfDist = 0.5*(radius0+radius1); vec_sum(halfDist, c0c1, &Arg1->p); vec_sum(-halfDist, c0c1, &Arg2->p); /* vec_copy(Arg1->oldPos, &correction); vec_sum(-1.0, Arg1->p, &correction); normalize(&correction); vec_sum(halfDist, correction, &Arg1->p); vec_copy(Arg2->oldPos, &correction); vec_sum(-1.0, Arg2->p, &correction); normalize(&correction); vec_sum(halfDist, correction, &Arg2->p);*/ Arg1->ApplyEffect(v1f, Arg1->p, PE_CONTACT, Arg2); Arg2->ApplyEffect(v2f, Arg2->p, PE_CONTACT, Arg1); collision = true; // Activate the OnCollisionDetected event in the end } } break; default: break; } } break; default: break; } if (collision) { Arg1->OnCollisionDetected(Arg2); Arg2->OnCollisionDetected(Arg1); } node2index++; //cOSecNode = cOSecNode->nxt; cOSecNode++; } node1index++; //cONode = cONode->nxt; cONode++; } // Now perform the status update //#ifndef SDL #if MAX_PROCESSORS > 1 int remain = objects.size() % MAX_PROCESSORS; std::list::iterator limit; limit = objects.begin(); for (int processor = 0; processor < MAX_PROCESSORS; processor++) { update_blocks[processor].start = limit; int topstop = processor < MAX_PROCESSORS-1 ? (objects.size() / MAX_PROCESSORS) : (objects.size() / MAX_PROCESSORS) +remain; for (int move_it = 0; move_it < topstop; move_it++) limit++; update_blocks[processor].stop = limit; update_blocks[processor].objects = objects; update_blocks[processor].t_elapsed = t_elapsed; update_blocks[processor].numThread = topstop; update_children[processor] = SDL_CreateThread(thread_Update, &update_blocks[processor]); } for (int processor = 0; processor < MAX_PROCESSORS; processor++) { SDL_WaitThread(update_children[processor], NULL); } #else cONode = objects.begin(); while (cONode != objects.end()) { PO_ptr = *cONode; PO_ptr->Update(t_elapsed); PO_ptr->ResetAccels(); cONode++; } #endif // Resolve collisions with meshes after sphere-sphere collisions // to prevent the spheres from crossing the mesh std::list::iterator meshNode; meshNode = meshes.begin(); while (meshNode != meshes.end()) { pMesh* Mesh_ptr = *meshNode; int arg2index = 0; cONode = objects.begin(); while (cONode != objects.end()) { PO_ptr = *cONode; triangle ** triangles = (triangle ** ) Mesh_ptr->cGeometryData; //pMesh * tm0 = (pMesh*) Arg1; switch(PO_ptr->cGeometry) { case CG_PT: { unsigned ctriangle = 0; unsigned currCollStatus = COLR_NOCOLLISION; vector3 correction; while (triangles[ctriangle] && (currCollStatus == COLR_NOCOLLISION)) { vector3* rayTrTestDir = rayTrDir? rayTrDir : triangles[ctriangle]->n; unsigned prevCollStatus = Mesh_ptr->collStatuses[ctriangle][arg2index]; currCollStatus = col_ray_tr(PO_ptr->p, rayTrTestDir,// triangles[ctriangle]->n, *triangles[ctriangle], &correction); //if (currCollStatus || prevCollStatus) //printf("Collstatus[%d]: now %d vs %d (%3.2f,%3.2f,%3.2f)\n", //ctriangle, currCollStatus, prevCollStatus, Arg2->p.x, //Arg2->p.y, Arg2->p.z); if (Mesh_ptr->objectInteractionEffect == PE_C_IMMOVABLE) { if ((currCollStatus == COLR_UNDER) && ((prevCollStatus != COLR_NOCOLLISION)&&(prevCollStatus != COLR_UNDER))) { //if (Arg1->objectInteractionEffect == PE_C_IMMOVABLE) PO_ptr->ApplyEffect(*triangles[ctriangle]->n, correction, Mesh_ptr->objectInteractionEffect, Mesh_ptr); //else vec_copy(correction, &PO_ptr->p); currCollStatus = COLR_EXACT; //collision = true; // Activate the OnCollisionDetected event in the end } } else { if (currCollStatus == COLR_UNDER) { PO_ptr->ApplyEffect(Mesh_ptr->objectInteractionParam, correction, Mesh_ptr->objectInteractionEffect, Mesh_ptr); //collision = true; } } Mesh_ptr->collStatuses[ctriangle][arg2index] = currCollStatus; ctriangle++; } } break; case CG_SPHERE: { unsigned ctriangle = 0; unsigned currCollStatus = COLR_NOCOLLISION; vector3 correction; float radius = * (float *) PO_ptr->cGeometryData; while (triangles[ctriangle] && (currCollStatus == COLR_NOCOLLISION)) { vector3* rayTrTestDir = rayTrDir? rayTrDir : triangles[ctriangle]->n; unsigned prevCollStatus = Mesh_ptr->collStatuses[ctriangle][arg2index]; vector3 spherePt; vec_copy(PO_ptr->p, &spherePt); vec_sum(-radius, *rayTrTestDir, &spherePt); currCollStatus = col_ray_tr(spherePt, rayTrTestDir, *triangles[ctriangle], &correction); vec_sum(radius, *rayTrTestDir, &correction); //if (currCollStatus || prevCollStatus) //printf("Collstatus[%d]: now %d vs %d (%3.2f,%3.2f,%3.2f)\n", //ctriangle, currCollStatus, prevCollStatus, Arg2->p.x, //Arg2->p.y, Arg2->p.z); if ((currCollStatus == COLR_UNDER) && ((prevCollStatus != COLR_NOCOLLISION)))//&&(prevCollStatus != COLR_UNDER))) { if (Mesh_ptr->objectInteractionEffect == PE_C_IMMOVABLE) { PO_ptr->ApplyEffect(*triangles[ctriangle]->n, correction, Mesh_ptr->objectInteractionEffect, Mesh_ptr); vec_copy(correction, &PO_ptr->p); currCollStatus = COLR_EXACT; } else { PO_ptr->ApplyEffect(Mesh_ptr->objectInteractionParam, correction, Mesh_ptr->objectInteractionEffect, Mesh_ptr); } //vec_copy(correction, &Arg2->p); //collision = true; // Activate the OnCollisionDetected event in the end } Mesh_ptr->collStatuses[ctriangle][arg2index] = currCollStatus; ctriangle++; } } break; case CG_MESH: // Not handled yet break; default: printf("Case not handled.. mesh vs %d\n", PO_ptr->cGeometry); break; cONode++; } cONode++; arg2index++; } meshNode++; } //#else //#endif } int thread_Update(void* data) { mjUpdateBlock* block = (mjUpdateBlock*) data; std::list::iterator cNode; cNode = block->start; while (cNode != block->stop) { IPhysicalObject* PO_ptr; PO_ptr = *cNode; PO_ptr->Update(block->t_elapsed); cNode++; } //printf("I am thread %d\n", block->numThread); return 1; } bool mjPhysics::RemoveObject(IPhysicalObject * physObject) { bool found = false; //list_node * cNode; std::list::iterator cNode; std::list::iterator mNode; unsigned posInList = 0; // Remove all temporary effects RemoveEffectsForObject(physObject); // Search for object in objects list cNode = objects.begin(); while(cNode != objects.end() && !found) { if ( physObject == *cNode) { cNode = objects.erase(cNode); found = true; } else cNode++; posInList++; } // Search for object in meshes list mNode = meshes.begin(); if (found) { while(mNode != meshes.end()) { //list_node * prevNode; if (physObject == (IPhysicalObject *) *mNode) { mNode = meshes.erase(mNode); } else { // Remove object's space from the mesh pMesh * cMesh = *mNode; cMesh->OnObjectDeleted(posInList-1, objects); } mNode++; } } return found; } void mjPhysics::SetDragFactor(float newDragFactor) { //list_node * cObj; std::list::iterator cObj; IPhysicalObject * PO_ptr; dragFactor = newDragFactor; cObj = objects.begin(); while (cObj != objects.end()) { //PO_ptr = (IPhysicalObject *) cObj->data; PO_ptr = *cObj; PO_ptr->dragFactor = dragFactor; //cObj = cObj->nxt; cObj++; } }