# Home    # nevrax.com   
Nevrax
Nevrax.org
#News
#Mailing-list
#Documentation
#CVS
#Bugs
#License
Docs
 
Documentation  
Main Page   Namespace List   Class Hierarchy   Alphabetical List   Compound List   File List   Namespace Members   Compound Members   File Members   Related Pages   Search  

primitive_world_image.cpp

Go to the documentation of this file.
00001 
00007 /* Copyright, 2001 Nevrax Ltd.
00008  *
00009  * This file is part of NEVRAX NEL.
00010  * NEVRAX NEL is free software; you can redistribute it and/or modify
00011  * it under the terms of the GNU General Public License as published by
00012  * the Free Software Foundation; either version 2, or (at your option)
00013  * any later version.
00014 
00015  * NEVRAX NEL is distributed in the hope that it will be useful, but
00016  * WITHOUT ANY WARRANTY; without even the implied warranty of
00017  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
00018  * General Public License for more details.
00019 
00020  * You should have received a copy of the GNU General Public License
00021  * along with NEVRAX NEL; see the file COPYING. If not, write to the
00022  * Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston,
00023  * MA 02111-1307, USA.
00024  */
00025 
00026 #include "stdpacs.h"
00027 
00028 #include "nel/misc/hierarchical_timer.h"
00029 
00030 #include "pacs/primitive_world_image.h"
00031 #include "pacs/move_primitive.h"
00032 #include "pacs/move_element.h"
00033 
00034 using namespace NLMISC;
00035 
00036 
00037 namespace NLPACS 
00038 {
00039 
00040 // ***************************************************************************
00041 
00042 CPrimitiveWorldImage::CPrimitiveWorldImage()
00043 {
00044         // Set to NULL
00045         for (uint i=0; i<4; i++)
00046                 _MoveElement[i]=NULL;
00047 
00048         _DynamicFlags=0;
00049         _BBXMin=-FLT_MAX;
00050         _BBXMax=-FLT_MAX;
00051         _BBYMin=-FLT_MAX;
00052         _BBYMax=-FLT_MAX;
00053 }
00054 
00055 // ***************************************************************************
00056 
00057 void CPrimitiveWorldImage::deleteIt (CMoveContainer &container, uint8 worldImage)
00058 {
00059         // Free the move elements       
00060         for (uint i=0; i<4; i++)
00061                 if (_MoveElement[i])
00062                         removeMoveElement (i, container, worldImage);
00063 }
00064 // ***************************************************************************
00065 
00066 void CPrimitiveWorldImage::copy (const CPrimitiveWorldImage& source)
00067 {
00068         // Copy
00069         this->operator=(source);
00070 
00071         // Reset some flags
00072         _DynamicFlags&=~InModifiedListFlag;
00073 
00074         // Pointer into the 4 possibles sorted lists of movable primitives. Must be NULL
00075         for (uint i=0; i<4; i++)
00076                 _MoveElement[i]=NULL;
00077 }
00078 
00079 // ***************************************************************************
00080 
00081 bool CPrimitiveWorldImage::evalCollision (CPrimitiveWorldImage& other, CCollisionDesc& desc, double timeMin, double timeMax, uint32 testTime, 
00082                                                                         uint32 maxTestIteration, double &firstContactTime, double &lastContactTime, CMovePrimitive& primitive,
00083                                                                         CMovePrimitive& otherPrimitive)
00084 {
00085 //      H_AUTO(PACS_PWI_evalCollision_long);
00086 
00087         // Mask test
00088         if (( (primitive.getCollisionMaskInternal() & otherPrimitive.getOcclusionMaskInternal())  == 0) && 
00089                 ( (primitive.getOcclusionMaskInternal() & otherPrimitive.getCollisionMaskInternal())  == 0))
00090                 return false;
00091 
00092         // Test time
00093         if ( (!primitive.checkTestTime (testTime, maxTestIteration)) || (!otherPrimitive.checkTestTime (testTime, maxTestIteration)) )
00094                 return false;
00095 
00096         // Clear time min time max
00097         firstContactTime=FLT_MAX;
00098         lastContactTime=-FLT_MAX;
00099 
00100         // Switch the good test
00101         switch (primitive.getPrimitiveTypeInternal())
00102         {
00103 
00104         // Static box over...
00105         case UMovePrimitive::_2DOrientedBox:
00106                 {
00107                         // Switch second type
00108                         switch (otherPrimitive.getPrimitiveTypeInternal())
00109                         {
00110                         
00111                         // Static box over movable box
00112                         case UMovePrimitive::_2DOrientedBox:
00113                                 // Make the test
00114                                 return evalCollisionOBoverOB (other, desc, timeMin, timeMax, firstContactTime, lastContactTime, primitive, otherPrimitive);
00115 
00116                         // Static box over movable cylinder
00117                         case UMovePrimitive::_2DOrientedCylinder:
00118                                 // Make the test
00119                                 return evalCollisionOBoverOC (other, desc, timeMin, timeMax, firstContactTime, lastContactTime, primitive, otherPrimitive);
00120 
00121                         default:
00122                         // Should not go here
00123                         nlstop;
00124                         }
00125                 }
00126 
00127         // Static box over...
00128         case UMovePrimitive::_2DOrientedCylinder:
00129                 {
00130                         // Switch second type
00131                         switch (otherPrimitive.getPrimitiveTypeInternal())
00132                         {
00133                         
00134                         // Static box over movable box
00135                         case UMovePrimitive::_2DOrientedBox:
00136                                 {
00137                                         // Make the test
00138                                         bool collid=other.evalCollisionOBoverOC (*this, desc, timeMin, timeMax, firstContactTime, lastContactTime, otherPrimitive, 
00139                                                 primitive);
00140                                         if (collid)
00141                                                 desc.XChgContactNormals ();
00142                                         return collid;
00143                                 }
00144 
00145                         // Static box over movable cylinder
00146                         case UMovePrimitive::_2DOrientedCylinder:
00147                                 // Make the test
00148                                 return evalCollisionOCoverOC (other, desc, timeMin, timeMax, firstContactTime, lastContactTime, primitive, otherPrimitive);
00149 
00150                         default:
00151                         // Should not go here
00152                         nlstop;
00153                         }
00154                 }
00155 
00156         default:
00157                 // Should not go here
00158                 nlstop;
00159         }       
00160 
00161         return false;
00162 }
00163 
00164 // ***************************************************************************
00165 
00166 const TCollisionSurfaceDescVector *CPrimitiveWorldImage::evalCollision (CGlobalRetriever &retriever, CCollisionSurfaceTemp& surfaceTemp, 
00167                                                                                                                                   uint32 testTime, uint32 maxTestIteration, CMovePrimitive& primitive)
00168 {
00169 //      H_AUTO(PACS_PWI_evalCollision_short);
00170 
00171         // Test time
00172         if (!primitive.checkTestTime (testTime, maxTestIteration))
00173                 return NULL;
00174 
00175         // Switch the good test
00176         if (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox)
00177         {
00178                 // Local I
00179                 CVector locI ((float)(_OBData.EdgeDirectionX[0]*primitive.getLength(0)/2.0), (float)(_OBData.EdgeDirectionY[0]*primitive.getLength(1)/2.0), 0);
00180 
00181                 // Local J
00182                 CVector locJ ((float)(_OBData.EdgeDirectionX[1]*primitive.getLength(0)/2.0), (float)(_OBData.EdgeDirectionY[1]*primitive.getLength(1)/2.0), 0);
00183 
00184                 // Test
00185                 return retriever.testBBoxMove (_Position.getGlobalPos (), _DeltaPosition, locI, locJ, surfaceTemp);
00186         }
00187         else
00188         {
00189                 // Check
00190                 nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
00191 
00192                 // Test
00193                 //nlinfo ("1) %f %f %f\n", _DeltaPosition.x, _DeltaPosition.y, _DeltaPosition.z);
00194                 
00195                 return retriever.testCylinderMove (_Position.getGlobalPos (), _DeltaPosition, primitive.getRadiusInternal(), surfaceTemp);
00196         }
00197 }
00198 
00199 // ***************************************************************************
00200 
00201 void CPrimitiveWorldImage::doMove (CGlobalRetriever &retriever, CCollisionSurfaceTemp& surfaceTemp, double originalMax, double finalMax, bool keepZ /*= false*/)
00202 {
00203 //      H_AUTO(PACS_PWI_doMove_long);
00204 
00205         // Time to avance
00206         double ratio;
00207         if (finalMax!=originalMax)
00208                 ratio=(finalMax-_InitTime)/(originalMax-_InitTime);
00209         else
00210                 ratio=1;
00211 
00212         // Make the move
00213         if (!keepZ)
00214         {       
00215                 _Position.setGlobalPos (retriever.doMove(_Position.getGlobalPos(), _DeltaPosition, (float)ratio, surfaceTemp, false), retriever);
00216         }
00217         else
00218         {
00219                 _Position.setGlobalPosKeepZ(retriever.doMove(_Position.getGlobalPos(), _DeltaPosition, (float)ratio, surfaceTemp, false), retriever);
00220         }
00221                 
00222 
00223         // Final position
00224         _InitTime=finalMax;
00225 }
00226 
00227 // ***************************************************************************
00228 
00229 void CPrimitiveWorldImage::doMove (double timeMax)
00230 {
00231 //      H_AUTO(PACS_PWI_doMove_short);
00232 
00233         // Make the move
00234         _Position.setPos (_Position.getPos ()+_Speed*(timeMax-_InitTime));
00235 
00236         // Final position
00237         _InitTime=timeMax;
00238 }
00239 
00240 // ***************************************************************************
00241 
00242 bool CPrimitiveWorldImage::evalCollisionOBoverOB (CPrimitiveWorldImage& other, CCollisionDesc& desc, double timeMin, double timeMax, 
00243                                                                                         double &firstContactTime, double &lastContactTime, CMovePrimitive& primitive,
00244                                                                                    CMovePrimitive& otherPrimitive)
00245 {
00246         // Checks
00247         nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
00248         nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
00249 
00250         // Find a collision
00251         bool find=false;
00252 
00253         // Best time
00254         desc.ContactTime=FLT_MAX;
00255 
00256         // Timemin
00257         double _timeMax=-FLT_MAX;
00258 
00259         // Check movable points over the edge
00260         uint pt;
00261         uint seg;
00262         for (pt=0; pt<4; pt++)
00263         for (seg=0; seg<4; seg++)
00264         {
00265                 // Get collision time of the point over the segment
00266                 CCollisionDesc d;
00267                 if ( evalCollisionPoverS (other, d, pt, seg, primitive, otherPrimitive) )
00268                 {
00269                         // Find
00270                         find=true;
00271 
00272                         // Best time ?
00273                         if (d.ContactTime<desc.ContactTime)
00274                         {
00275                                 // This is the new descriptor
00276                                 desc=d;
00277                         }
00278 
00279                         // Best max time ?
00280                         if (d.ContactTime>_timeMax)
00281                         {
00282                                 // This is the new max time
00283                                 _timeMax=d.ContactTime;
00284                         }
00285                 }
00286         }
00287 
00288         // Check static points over the movable box
00289         for (pt=0; pt<4; pt++)
00290         for (seg=0; seg<4; seg++)
00291         {
00292                 // Get collision time of the point over the segment
00293                 CCollisionDesc d;
00294                 if (other.evalCollisionPoverS (*this, d, pt, seg, primitive, otherPrimitive))
00295                 {
00296                         // Find
00297                         find=true;
00298 
00299                         // Best time ?
00300                         if (d.ContactTime<desc.ContactTime)
00301                         {
00302                                 // This is the new descriptor
00303                                 desc=d;
00304                         }
00305 
00306                         // Best max time ?
00307                         if (d.ContactTime>_timeMax)
00308                         {
00309                                 // This is the new max time
00310                                 _timeMax=d.ContactTime;
00311                         }
00312                 }
00313         }
00314 
00315         if (find)
00316         {
00317                 // First last contact time
00318                 firstContactTime=desc.ContactTime;
00319                 lastContactTime=_timeMax;
00320 
00321                 // Half time
00322                 //double halfTime = (_timeMax+desc.ContactTime)/2.0;
00323 
00324                 // Collision in the past ?
00325                 //if (timeMin > halfTime)
00326                 if (timeMin > _timeMax)
00327                         // yes, abort
00328                         return false;
00329 
00330                 // Collision not in the future ?
00331                 if (timeMax>desc.ContactTime)
00332                 {
00333                         // Clamp time
00334                         if (desc.ContactTime<timeMin)
00335                                 desc.ContactTime=timeMin;
00336 
00337                         // yes, found it
00338                         return true;
00339                 }
00340         }
00341 
00342         // No collision found
00343         return false;
00344 }
00345 
00346 // ***************************************************************************
00347 
00348 bool CPrimitiveWorldImage::evalCollisionOBoverOC (CPrimitiveWorldImage& other, CCollisionDesc& desc, double timeMin, double timeMax, 
00349                                                                                         double &firstContactTime, double &lastContactTime, CMovePrimitive& primitive,
00350                                                                                    CMovePrimitive& otherPrimitive)
00351 {
00352         // Checks
00353         nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
00354         nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
00355 
00356         // Find a collision
00357         bool find=false;
00358 
00359         // Best time
00360         desc.ContactTime=FLT_MAX;
00361 
00362         // time min clip
00363         double _timeMax = -FLT_MAX;
00364 
00365         // Check movable points over the cylinder
00366         uint pt;
00367         for (pt=0; pt<4; pt++)
00368         {
00369                 // Get collision time of the point over the segment
00370                 CCollisionDesc d;
00371                 double firstContactTime;
00372                 double lastContactTime;
00373                 if (evalCollisionPoverOC (other, d, pt, firstContactTime, lastContactTime, primitive, otherPrimitive))
00374                 {
00375                         // Found
00376                         find=true;
00377 
00378                         // Best time ?
00379                         if (firstContactTime<desc.ContactTime)
00380                         {
00381                                 // This is the new descriptor
00382                                 desc=d;
00383                         }
00384 
00385                         // Best max time ?
00386                         if (lastContactTime>_timeMax)
00387                         {
00388                                 // New max time
00389                                 _timeMax=lastContactTime;
00390                         }
00391                 }
00392         }
00393 
00394         // Check static points over the movable box
00395         uint seg;
00396         for (seg=0; seg<4; seg++)
00397         {
00398                 // Get collision time of the point over the segment
00399                 CCollisionDesc d;
00400                 if (evalCollisionSoverOC (other, d, seg, primitive, otherPrimitive))
00401                 {
00402                         // Found
00403                         find=true;
00404 
00405                         // Best time ?
00406                         if (d.ContactTime<desc.ContactTime)
00407                         {
00408                                 // This is the new descriptor
00409                                 desc=d;
00410                         }
00411 
00412                         // Best max time ?
00413                         if (d.ContactTime>_timeMax)
00414                         {
00415                                 // New max time
00416                                 _timeMax=d.ContactTime;
00417                         }
00418                 }
00419         }
00420 
00421         if (find)
00422         {
00423                 // First last contact time
00424                 firstContactTime=desc.ContactTime;
00425                 lastContactTime=_timeMax;
00426 
00427                 // Half time
00428                 //double halfTime = (_timeMax+desc.ContactTime)/2.0;
00429 
00430                 // Collision in the past ?
00431                 //if (timeMin > halfTime)
00432                 if (timeMin > _timeMax)
00433                         // yes, abort
00434                         return false;
00435 
00436                 // Collision not in the future ?
00437                 if (timeMax>desc.ContactTime)
00438                 {
00439                         // Clamp time
00440                         if (desc.ContactTime<timeMin)
00441                                 desc.ContactTime=timeMin;
00442 
00443                         // yes, found it
00444                         return true;
00445                 }
00446         }
00447 
00448         // No collision found
00449         return false;
00450 }
00451 
00452 // ***************************************************************************
00453 
00454 bool CPrimitiveWorldImage::evalCollisionPoverS (CPrimitiveWorldImage& other, CCollisionDesc& desc, uint numPoint, uint numSeg, 
00455                                                                                                 CMovePrimitive& primitive, CMovePrimitive& otherPrimitive)
00456 {
00457         // Checks
00458         nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
00459         nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
00460 
00461         // Some constants
00462         const double normalSegX=other._OBData.EdgeDirectionY[numSeg];
00463         const double normalSegY=-other._OBData.EdgeDirectionX[numSeg];
00464 
00465         // Relative speed
00466         const double speedX=other._Speed.x-_Speed.x;
00467         const double speedY=other._Speed.y-_Speed.y;
00468 
00469         // Dot product with the plan tangeante
00470         double dotProd= speedX*normalSegX + speedY*normalSegY;
00471         //if ( dotProd > 0 )
00472         if ( dotProd != 0 )
00473         {
00474                 // Time of the collision
00475                 double time= (normalSegX*(_OBData.PointPosX[numPoint] - other._OBData.PointPosX[numSeg]) + 
00476                         normalSegY*(_OBData.PointPosY[numPoint] - other._OBData.PointPosY[numSeg])) / dotProd;
00477 
00478                 // Position of segment point at collision time
00479                 const double segPosX= other._OBData.PointPosX[numSeg] + other._Speed.x*time;
00480                 const double segPosY= other._OBData.PointPosY[numSeg] + other._Speed.y*time;
00481 
00482                 // Position of the point at collision time
00483                 const double ptPosX= _OBData.PointPosX[numPoint] + _Speed.x*time;
00484                 const double ptPosY= _OBData.PointPosY[numPoint] + _Speed.y*time;
00485 
00486                 // Direction of the collision on the segment
00487                 const double dirX= ptPosX - segPosX;
00488                 const double dirY= ptPosY - segPosY;
00489 
00490                 // Length of this vector
00491                 const double length= dirY*normalSegX - dirX*normalSegY;
00492 
00493                 // Included ?
00494                 if ( ( length >= 0 ) && ( length <= otherPrimitive.getLength(numSeg&1) ) )
00495                 {
00496                         // 2d Collid checked... Now check height
00497                         
00498                         // Pos Z
00499                         const double pointSegZ=other._3dInitPosition.z;
00500                         const double segPosZ= pointSegZ + other._Speed.z*time;
00501 
00502                         // Some constants
00503                         const double pointZ=_3dInitPosition.z;
00504                         const double ptPosZ= pointZ + _Speed.z*time;
00505 
00506                         // Included ?
00507                         if ( (ptPosZ <= segPosZ + otherPrimitive.getHeightInternal()) && (ptPosZ + primitive.getHeightInternal() >= segPosZ) )
00508                         {
00509                                 // Ok Collision, fill the result
00510                                 
00511                                 // Time
00512                                 desc.ContactTime=time;
00513 
00514                                 // Position
00515                                 desc.ContactPosition.x=ptPosX;
00516                                 desc.ContactPosition.y=ptPosY;
00517                                 desc.ContactPosition.z=std::max (segPosZ, ptPosZ);
00518 
00519                                 // Seg box normal
00520                                 desc.ContactNormal1.x=normalSegX;
00521                                 desc.ContactNormal1.y=normalSegY;
00522                                 desc.ContactNormal1.z=0;
00523                                 desc.ContactNormal0.x=-desc.ContactNormal1.x;
00524                                 desc.ContactNormal0.y=-desc.ContactNormal1.y;
00525                                 desc.ContactNormal0.z=0;
00526 
00527                                 // End
00528                                 return true;
00529                         }
00530                 }
00531         }
00532 
00533         // No collision
00534         return false;
00535 }
00536 
00537 // ***************************************************************************
00538 
00539 inline uint secondDegree (double a, double b, double c, double& s0, double& s1)
00540 {
00541         double d=b*b-4.f*a*c;
00542         if (d>0)
00543         {
00544                 // sqrt d
00545                 d=(double)sqrt (d);
00546 
00547                 // 1 / 2a
00548                 a=0.5f/a;
00549 
00550                 // 2 solutions
00551                 s0 = (-b-d)*a;
00552                 s1 = (-b+d)*a;
00553 
00554                 return 2;
00555         }
00556         else if (d<0)
00557         {
00558                 // No solution
00559                 return 0;
00560         }
00561         else
00562         {
00563                 // 1 solution
00564                 s0 = -b/(2.f*a);
00565 
00566                 return 1;
00567         }
00568 }
00569 
00570 // ***************************************************************************
00571 
00572 bool CPrimitiveWorldImage::evalCollisionPoverOC (CPrimitiveWorldImage& other, CCollisionDesc& desc, uint numPoint, 
00573                                                                                    double &firstContactTime, double &lastContactTime, CMovePrimitive& primitive,
00574                                                                                    CMovePrimitive& otherPrimitive)
00575 {
00576         // Checks
00577         nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
00578         nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
00579 
00580         /* Point Equ:
00581          * p(t) = p0 + v0*(t - t0)
00582          *
00583          * Cylinder center Equ:
00584          * p'(t) = p'0 + v'0*(t - t'0)
00585          *
00586          * Find t for this equation:
00587          * Rē = Normē (p(t) - p'(t))
00588          * Rē = Normē ( p0 + v0 ( t - t0 ) - p'0 - v'0 ( t - t'0 ) )
00589          *
00590          * A = p0 - v0*t0 - p'0 + v'0*t'0
00591          * B = (v0 - v'0)
00592          *
00593          * Normē (B)*tē + 2*(A.B)*t + Normē (A) - Rē = 0
00594          *
00595          * a = Normē (B)
00596          * b = 2*(A.B)
00597          * c = Normē (A) - Rē
00598          *
00599          * a*tē + b*t + c = 0
00600          */
00601 
00602         // Let's go
00603         const double _Ax = _OBData.PointPosX[numPoint] - other._3dInitPosition.x;
00604         const double _Ay = _OBData.PointPosY[numPoint] - other._3dInitPosition.y;
00605         const double _Bx = _Speed.x - other._Speed.x;
00606         const double _By = _Speed.y - other._Speed.y;
00607 
00608         // Eval system
00609         double s0, s1;
00610         double squareRadius=otherPrimitive.getRadiusInternal()*otherPrimitive.getRadiusInternal();
00611         uint numSolution=secondDegree (_Bx*_Bx+_By*_By, 2.f*(_Ax*_Bx+_Ay*_By), _Ax*_Ax+_Ay*_Ay-squareRadius, s0, s1);
00612         if (numSolution!=0)
00613         {
00614                 // time
00615                 double time;
00616 
00617                 // Collision time
00618                 if (numSolution==1)
00619                 {
00620                         firstContactTime=s0;
00621                         lastContactTime=s0;
00622                 }
00623                 else
00624                 {
00625                         // First and last time
00626                         if (s0<s1)
00627                         {
00628                                 firstContactTime=s0;
00629                                 lastContactTime=s1;
00630                         }
00631                         else
00632                         {
00633                                 firstContactTime=s1;
00634                                 lastContactTime=s0;
00635                         }
00636                 }
00637                 time=firstContactTime;
00638 
00639                 // Pos Z
00640                 const double pointCylZ=other._3dInitPosition.z;
00641                 const double cylPosZ= pointCylZ + other._Speed.z*time;
00642 
00643                 // Some constants
00644                 const double pointZ=_3dInitPosition.z;
00645                 const double ptPosZ= pointZ + _Speed.z*time;
00646 
00647                 // Z Included ?
00648                 if ( (ptPosZ <= cylPosZ + otherPrimitive.getHeightInternal()) && (ptPosZ + primitive.getHeightInternal() >= cylPosZ) )
00649                 {
00650                         // Ok Collision, fill the result
00651                         
00652                         // Time
00653                         desc.ContactTime=time;
00654 
00655                         // Point position
00656                         const double ptPosX= _OBData.PointPosX[numPoint] + _Speed.x*time;
00657                         const double ptPosY= _OBData.PointPosY[numPoint] + _Speed.y*time;
00658 
00659                         // Cylinder position
00660                         const double cylPosX= other._3dInitPosition.x + other._Speed.x*time;
00661                         const double cylPosY= other._3dInitPosition.y + other._Speed.y*time;
00662 
00663                         // Position
00664                         desc.ContactPosition.x=ptPosX;
00665                         desc.ContactPosition.y=ptPosY;
00666                         desc.ContactPosition.z=std::max (cylPosZ, ptPosZ);
00667 
00668                         // Cylinder normal
00669                         desc.ContactNormal1.x=ptPosX-cylPosX;
00670                         desc.ContactNormal1.y=ptPosY-cylPosY;
00671                         desc.ContactNormal1.z=0;
00672                         desc.ContactNormal1.normalize ();
00673                         desc.ContactNormal0.x=-desc.ContactNormal1.x;
00674                         desc.ContactNormal0.y=-desc.ContactNormal1.y;
00675                         desc.ContactNormal0.z=0;
00676 
00677                         // End
00678                         return true;
00679                 }
00680         }
00681 
00682         // No collision
00683         return false;
00684 }
00685 
00686 // ***************************************************************************
00687 
00688 bool CPrimitiveWorldImage::evalCollisionSoverOC (CPrimitiveWorldImage& other, CCollisionDesc& desc, uint numSeg, CMovePrimitive& primitive,
00689                                                                                    CMovePrimitive& otherPrimitive)
00690 {
00691         // Checks
00692         nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
00693         nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
00694 
00695         // Some constants
00696         const double normalSegX=_OBData.EdgeDirectionY[numSeg];
00697         const double normalSegY=-_OBData.EdgeDirectionX[numSeg];
00698 
00699         // Relative speed
00700         const double speedX=other._Speed.x-_Speed.x;
00701         const double speedY=other._Speed.y-_Speed.y;
00702 
00703         // Dot product with the plan tangeante
00704         double dotProd= speedX*normalSegX + speedY*normalSegY;
00705         //if ( dotProd < 0 )
00706         if ( dotProd !=0 )
00707         {
00708                 // Time of the collision
00709                 double time= (otherPrimitive.getRadiusInternal() + normalSegX*(_OBData.PointPosX[numSeg] - other._3dInitPosition.x ) + 
00710                         normalSegY*(_OBData.PointPosY[numSeg] - other._3dInitPosition.y ) ) / dotProd;
00711 
00712                 // Position of segment point at collision time
00713                 const double segPosX= _OBData.PointPosX[numSeg] + _Speed.x*time;
00714                 const double segPosY= _OBData.PointPosY[numSeg] + _Speed.y*time;
00715 
00716                 // Position of the cylinder at collision time
00717                 const double cylPosX= other._3dInitPosition.x + _Speed.x*time;
00718                 const double cylPosY= other._3dInitPosition.y + _Speed.y*time;
00719 
00720                 // Position de contact
00721                 const double contactX= cylPosX - normalSegX*otherPrimitive.getRadiusInternal();
00722                 const double contactY= cylPosY - normalSegY*otherPrimitive.getRadiusInternal();
00723 
00724                 // Direction of the collision on the segment
00725                 const double dirX= contactX - segPosX;
00726                 const double dirY= contactY - segPosY;
00727 
00728                 // Length of this vector
00729                 const double length= dirY*normalSegX - dirX*normalSegY;
00730 
00731                 // Included ?
00732                 if ( ( length >= 0 ) && ( length <= primitive.getLength (numSeg&1) ) )
00733                 {
00734                         // 2d Collid checked... Now check height
00735                         
00736                         // Pos Z
00737                         const double segPosZ= _3dInitPosition.z + _Speed.z*time;
00738 
00739                         // Some constants
00740                         const double cylPosZ= other._3dInitPosition.z + other._Speed.z*time;
00741 
00742                         // Included ?
00743                         if ( (cylPosZ <= segPosZ + primitive.getHeightInternal() ) && (cylPosZ + otherPrimitive.getHeightInternal() >= segPosZ) )
00744                         {
00745                                 // Ok Collision, fill the result
00746                                 
00747                                 // Time
00748                                 desc.ContactTime=time;
00749 
00750                                 // Position
00751                                 desc.ContactPosition.x=contactX;
00752                                 desc.ContactPosition.y=contactY;
00753                                 desc.ContactPosition.z=std::max (segPosZ, cylPosZ);
00754 
00755                                 // Segment normal
00756                                 desc.ContactNormal0.x=normalSegX;
00757                                 desc.ContactNormal0.y=normalSegY;
00758                                 desc.ContactNormal0.z=0;
00759 
00760                                 // Seg box normal
00761                                 desc.ContactNormal1.x=contactX-cylPosX;
00762                                 desc.ContactNormal1.y=contactY-cylPosY;
00763                                 desc.ContactNormal1.z=0;
00764                                 desc.ContactNormal1.normalize ();
00765 
00766                                 // End
00767                                 return true;
00768                         }
00769                 }
00770         }
00771 
00772         // No collision
00773         return false;
00774 }
00775 
00776 
00777 // ***************************************************************************
00778 
00779 bool CPrimitiveWorldImage::evalCollisionOCoverOC (CPrimitiveWorldImage& other, CCollisionDesc& desc, double timeMin, double timeMax, 
00780                                                                                         double &firstContactTime, double &lastContactTime, CMovePrimitive& primitive,
00781                                                                                    CMovePrimitive& otherPrimitive)
00782 {
00783         // Checks
00784         nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
00785         nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
00786 
00787 
00788         /* Cylinder0 center equ:
00789          * p(t) = p0 + v0*(t - t0)
00790          *
00791          * Cylinder1 center equ:
00792          * p'(t) = p'0 + v'0*(t - t'0)
00793          *
00794          * Find t for this equation:
00795          * (R + R')ē = Normē (p(t) - p'(t))
00796          * (R + R')ē = Normē ( p0 + v0 ( t - t0 ) - p'0 - v'0 ( t - t'0 ) )
00797          *
00798          * A = p0 - v0*t0 - p'0 + v'0*t'0
00799          * B = (v0 - v'0)
00800          *
00801          * Normē (B)*tē + 2*(A.B)*t + Normē (A) - (R + R')ē = 0
00802          *
00803          * a = Normē (B)
00804          * b = 2*(A.B)
00805          * c = Normē (A) - (R + R')ē
00806          *
00807          * a*tē + b*t + c = 0
00808          */
00809 
00810         // Let's go
00811         const double _Ax = _3dInitPosition.x - other._3dInitPosition.x;
00812         const double _Ay = _3dInitPosition.y - other._3dInitPosition.y;
00813         const double _Bx = _Speed.x - other._Speed.x;
00814         const double _By = _Speed.y - other._Speed.y;
00815 
00816         // Eval system
00817         double s0, s1;
00818         double radiusSquare=primitive.getRadiusInternal()+otherPrimitive.getRadiusInternal();
00819         radiusSquare*=radiusSquare;
00820         uint numSolution=secondDegree (_Bx*_Bx+_By*_By, 2.f*(_Ax*_Bx+_Ay*_By), _Ax*_Ax+_Ay*_Ay-radiusSquare, s0, s1);
00821         if (numSolution!=0)
00822         {
00823                 // time
00824                 double _timeMin, _timeMax;
00825 
00826                 // Collision time
00827                 if (numSolution==1)
00828                 {
00829                         _timeMin=s0;
00830                         _timeMax=s0;
00831                 }
00832                 else
00833                 {
00834                         // Time min and max
00835                         if (s0>s1)
00836                         {
00837                                 _timeMin=s1;
00838                                 _timeMax=s0;
00839                         }
00840                         else
00841                         {
00842                                 _timeMin=s0;
00843                                 _timeMax=s1;
00844                         }
00845                 }
00846 
00847                 // half time
00848                 //const double halfTime=(_timeMin+_timeMax)/2.0;
00849 
00850                 // Conatct time
00851                 firstContactTime=_timeMin;
00852                 lastContactTime=_timeMax;
00853 
00854                 // Clip time
00855                 if ((timeMin<_timeMax)&&(_timeMin<timeMax))
00856                 {
00857                         // Some constants
00858                         const double cyl0Time= _timeMin;
00859                         const double pointCyl0Z=_3dInitPosition.z;
00860                         const double cyl0PosZ= pointCyl0Z + _Speed.z*cyl0Time;
00861 
00862                         // Pos Z
00863                         const double cyl1Time= _timeMin;
00864                         const double pointCyl1Z=other._3dInitPosition.z;
00865                         const double cyl1PosZ= pointCyl1Z + other._Speed.z * cyl1Time;
00866 
00867                         // Z Included ?
00868                         if ( (cyl0PosZ <= cyl1PosZ + otherPrimitive.getHeightInternal() ) && (cyl0PosZ + primitive.getHeightInternal() >= cyl1PosZ) )
00869                         {
00870                                 // Ok Collision, fill the result
00871                                 
00872                                 // Time
00873                                 desc.ContactTime=std::max (_timeMin, timeMin);
00874 
00875                                 // Cylinder 0 position
00876                                 const double cyl0PosX= _3dInitPosition.x + _Speed.x*cyl0Time;
00877                                 const double cyl0PosY= _3dInitPosition.y + _Speed.y*cyl0Time;
00878 
00879                                 // Cylinder 1 position
00880                                 const double cyl1PosX= other._3dInitPosition.x + other._Speed.x*cyl1Time;
00881                                 const double cyl1PosY= other._3dInitPosition.y + other._Speed.y*cyl1Time;
00882 
00883                                 // First cylinder normal
00884                                 desc.ContactNormal0.x= cyl1PosX - cyl0PosX;
00885                                 desc.ContactNormal0.y= cyl1PosY - cyl0PosY;
00886                                 desc.ContactNormal0.z= 0;
00887                                 desc.ContactNormal0.normalize ();
00888 
00889                                 // Contact position
00890                                 desc.ContactPosition.x= desc.ContactNormal0.x*primitive.getRadiusInternal() + cyl0PosX;
00891                                 desc.ContactPosition.y= desc.ContactNormal0.y*primitive.getRadiusInternal() + cyl0PosY;
00892                                 desc.ContactPosition.z= std::max (cyl0PosZ, cyl1PosZ);
00893 
00894                                 // Second cylinder normal
00895                                 desc.ContactNormal1.x= -desc.ContactNormal0.x;
00896                                 desc.ContactNormal1.y= -desc.ContactNormal0.y;
00897                                 desc.ContactNormal1.z= 0;
00898 
00899                                 // End
00900                                 return true;
00901                         }
00902                 }
00903         }
00904 
00905         // No collision
00906         return false;
00907 }
00908 
00909 // ***************************************************************************
00910 
00911 void CPrimitiveWorldImage::precalcPos (CMovePrimitive &primitive)
00912 {
00913         // Type of the primitive
00914         uint type=primitive.getPrimitiveTypeInternal();
00915 
00916         // Box ?
00917         if (type==UMovePrimitive::_2DOrientedBox)
00918         {
00919                 // Calc cosinus and sinus
00920                 double cosinus=(double)cos(_OBData.Orientation);
00921                 double sinus=(double)sin(_OBData.Orientation);
00922 
00923                 // Size
00924                 double halfWidth=primitive.getLength (0)/2;
00925                 double halfDepth=primitive.getLength (1)/2;
00926 
00927                 // First point
00928                 _OBData.PointPosX[0]=cosinus*(-halfWidth)-sinus*(-halfDepth)+_3dInitPosition.x;
00929                 _OBData.PointPosY[0]=sinus*(-halfWidth)+cosinus*(-halfDepth)+_3dInitPosition.y;
00930 
00931                 // Second point
00932                 _OBData.PointPosX[1]=cosinus*halfWidth-sinus*(-halfDepth)+_3dInitPosition.x;
00933                 _OBData.PointPosY[1]=sinus*halfWidth+cosinus*(-halfDepth)+_3dInitPosition.y;
00934                 
00935                 // Third point
00936                 _OBData.PointPosX[2]=cosinus*halfWidth-sinus*halfDepth+_3dInitPosition.x;
00937                 _OBData.PointPosY[2]=sinus*halfWidth+cosinus*halfDepth+_3dInitPosition.y;
00938                 
00939                 // Fourth point
00940                 _OBData.PointPosX[3]=cosinus*(-halfWidth)-sinus*halfDepth+_3dInitPosition.x;
00941                 _OBData.PointPosY[3]=sinus*(-halfWidth)+cosinus*halfDepth+_3dInitPosition.y;
00942 
00943                 // Direction
00944                 double oneOverLength[2]= { 1 / primitive.getLength(0), 1 / primitive.getLength(1) };
00945 
00946                 // Direction
00947                 uint i;
00948                 for (i=0; i<4; i++)
00949                 {
00950                         // Next index
00951                         uint next=(i+1)&3;
00952                         double oneOver=oneOverLength[i&1];
00953 
00954                         // New direction
00955                         _OBData.EdgeDirectionX[i]=(_OBData.PointPosX[next] - _OBData.PointPosX[i])*oneOver;
00956                         _OBData.EdgeDirectionY[i]=(_OBData.PointPosY[next] - _OBData.PointPosY[i])*oneOver;
00957                 }
00958         }
00959         else
00960         {
00961                 // Should be a cylinder
00962                 nlassert (type==UMovePrimitive::_2DOrientedCylinder);
00963         }
00964 }
00965 
00966 // ***************************************************************************
00967 
00968 void CPrimitiveWorldImage::precalcBB (double beginTime, double endTime, CMovePrimitive &primitive)
00969 {
00970         // Type of the primitive
00971         uint type=primitive.getPrimitiveTypeInternal();
00972 
00973         // Box ?
00974         if (type==UMovePrimitive::_2DOrientedBox)
00975         {
00976                 // Orientation index
00977                 sint orient= (sint)(256.f*_OBData.Orientation/(2.f*NLMISC::Pi));
00978                 orient&=0xff;
00979                 orient>>=6;
00980                 nlassert (orient>=0);
00981                 nlassert (orient<4);
00982 
00983                 // Compute coordinates
00984                 _BBXMin=FLT_MAX;
00985                 _BBYMin=FLT_MAX;
00986                 _BBXMax=-FLT_MAX;
00987                 _BBYMax=-FLT_MAX;
00988 
00989                 for (uint i=0; i<4; i++)
00990                 {
00991                         if (_OBData.PointPosX[i]<_BBXMin)
00992                                 _BBXMin=_OBData.PointPosX[i];
00993                         if (_OBData.PointPosX[i]>_BBXMax)
00994                                 _BBXMax=_OBData.PointPosX[i];
00995                         if (_OBData.PointPosY[i]<_BBYMin)
00996                                 _BBYMin=_OBData.PointPosY[i];
00997                         if (_OBData.PointPosY[i]>_BBYMax)
00998                                 _BBYMax=_OBData.PointPosY[i];
00999                 }
01000                 _BBXMin=std::min (std::min (_BBXMin, _BBXMin+endTime*_Speed.x), _BBXMin+beginTime*_Speed.x);
01001                 _BBXMax=std::max (std::max (_BBXMax, _BBXMax+endTime*_Speed.x), _BBXMax+beginTime*_Speed.x);
01002                 _BBYMin=std::min (std::min (_BBYMin, _BBYMin+endTime*_Speed.y), _BBYMin+beginTime*_Speed.y);
01003                 _BBYMax=std::max (std::max (_BBYMax, _BBYMax+endTime*_Speed.y), _BBYMax+beginTime*_Speed.y);
01004 
01005 /*              
01006                 // This code is faster but buggy.. 
01007                 _BBXMin= _OBData.PointPosX[minX[orient]] + _Speed.x*beginTime;
01008                 _BBXMin= std::min (_BBXMin, _OBData.PointPosX[minX[orient]] + _Speed.x*endTime);
01009 
01010                 _BBYMin= _OBData.PointPosY[minY[orient]] + _Speed.y*beginTime;
01011                 _BBYMin= std::min (_BBYMin, _OBData.PointPosY[minY[orient]] + _Speed.y*endTime);
01012 
01013                 _BBXMax= _OBData.PointPosX[maxX[orient]] + _Speed.x*beginTime;
01014                 _BBXMax= std::max (_BBXMax, _OBData.PointPosX[maxX[orient]] + _Speed.x*endTime);
01015 
01016                 _BBYMax= _OBData.PointPosY[maxY[orient]] + _Speed.y*beginTime;
01017                 _BBYMax= std::max (_BBYMax, _OBData.PointPosY[maxY[orient]] + _Speed.y*endTime);*/
01018         }
01019         else
01020         {
01021                 // Should be a cylinder
01022                 nlassert (type==UMovePrimitive::_2DOrientedCylinder);
01023 
01024                 // Compute X coordinates
01025                 _BBXMin= _3dInitPosition.x + _Speed.x*beginTime;
01026                 _BBXMax= _3dInitPosition.x + _Speed.x*endTime;
01027                 if (_BBXMin>_BBXMax)
01028                 {
01029                         double tmp=_BBXMin;
01030                         _BBXMin=_BBXMax;
01031                         _BBXMax=tmp;
01032                 }
01033                 _BBXMin-=primitive.getRadiusInternal();
01034                 _BBXMax+=primitive.getRadiusInternal();
01035 
01036                 // Compute Y coordinates
01037                 _BBYMin= _3dInitPosition.y + _Speed.y*beginTime;
01038                 _BBYMax= _3dInitPosition.y + _Speed.y*endTime;
01039                 if (_BBYMin>_BBYMax)
01040                 {
01041                         double tmp=_BBYMin;
01042                         _BBYMin=_BBYMax;
01043                         _BBYMax=tmp;
01044                 }
01045                 _BBYMin-=primitive.getRadiusInternal();
01046                 _BBYMax+=primitive.getRadiusInternal();
01047         }
01048 
01049         // Delta position
01050         _DeltaPosition=_Speed*(endTime-beginTime);
01051 }
01052 
01053 // ***************************************************************************
01054 
01055 void CPrimitiveWorldImage::addMoveElement (CMoveCell& cell, uint16 x, uint16 y, double centerX, double centerY, CMovePrimitive *primitive,
01056                                                                                    CMoveContainer &container, uint8 worldImage)
01057 {
01058         // Find a free place
01059         uint slot;
01060         for (slot=0; slot<4; slot++)
01061         {
01062                 // Empty ?
01063                 if (_MoveElement[slot]==NULL)
01064                 {
01065                         // Primitive center
01066                         double cx=(_BBXMin+_BBXMax)/2.f;
01067 
01068                         // Allocate move element
01069                         _MoveElement[slot]=container.allocateMoveElement ();
01070                         _MoveElement[slot]->Primitive=primitive;
01071                         _MoveElement[slot]->X=x;
01072                         _MoveElement[slot]->Y=y;
01073 
01074                         // Insert in left or right ?
01075                         if (cx<centerX)
01076                                 // In the left
01077                                 cell.linkFirstX (_MoveElement[slot]);
01078                         else
01079                                 // In the right
01080                                 cell.linkLastX (_MoveElement[slot]);
01081 
01082                         /*// Insert in left or right ?
01083                         if (cy<centerY)
01084                                 // In the left
01085                                 cell.linkFirstY (_MoveElement[slot]);
01086                         else
01087                                 // In the right
01088                                 cell.linkLastY (_MoveElement[slot]);*/
01089 
01090                         // Move it
01091                          cell.updateSortedLists (_MoveElement[slot], worldImage);
01092 
01093                         // End
01094                         break;
01095                 }
01096         }
01097 }
01098 
01099 // ***************************************************************************
01100 
01101 void CPrimitiveWorldImage::addMoveElementendOfList (CMoveCell& cell, uint16 x, uint16 y, CMovePrimitive *primitive,
01102                                                                                                         CMoveContainer &container)
01103 {
01104         // Find a free place
01105         uint slot;
01106         for (slot=0; slot<4; slot++)
01107         {
01108                 // Empty ?
01109                 if (_MoveElement[slot]==NULL)
01110                 {
01111                         // Allocate move element
01112                         _MoveElement[slot]=container.allocateMoveElement ();
01113                         _MoveElement[slot]->Primitive=primitive;
01114                         _MoveElement[slot]->X=x;
01115                         _MoveElement[slot]->Y=y;
01116 
01117                         // In the right
01118                         cell.linkLastX (_MoveElement[slot]);
01119 
01120                         // End
01121                         break;
01122                 }
01123         }
01124 }
01125 
01126 // ***************************************************************************
01127 
01128 void CPrimitiveWorldImage::removeMoveElement (uint i, CMoveContainer &container, uint8 worldImage)
01129 {
01130         // Check
01131         nlassert ((i>=0)||(i<4));
01132         nlassert (_MoveElement[i]!=NULL);
01133 
01134         // Unlink the element
01135         container.unlinkMoveElement (_MoveElement[i], worldImage);
01136 
01137         // Free the move element
01138         container.freeMoveElement (_MoveElement[i]);
01139 
01140         // Set to NULL
01141         _MoveElement[i]=NULL;
01142 }
01143 
01144 // ***************************************************************************
01145 
01146 void CPrimitiveWorldImage::checkSortedList (uint8 worldImage)
01147 {
01148         // For the 4 elements
01149         for (uint i=0; i<4; i++)
01150         {
01151                 // element here ?
01152                 if (_MoveElement[i])
01153                 {
01154                         if (_MoveElement[i]->PreviousX)
01155                                 nlassertonce (_MoveElement[i]->PreviousX->Primitive->getWorldImage(worldImage)->_BBXMin <= _BBXMin);
01156                         if (_MoveElement[i]->NextX)
01157                                 nlassertonce (_BBXMin <= _MoveElement[i]->NextX->Primitive->getWorldImage(worldImage)->_BBXMin);
01158                 }
01159         }
01160 }
01161 
01162 // ***************************************************************************
01163 
01164 void CPrimitiveWorldImage::reaction (CPrimitiveWorldImage& second, const CCollisionDesc& desc, CGlobalRetriever* retriever,
01165                                                            CCollisionSurfaceTemp& surfaceTemp, bool collision, CMovePrimitive &primitive, 
01166                                                            CMovePrimitive &otherPrimitive, CMoveContainer *container, uint8 worldImage, uint8 secondWorldImage,
01167                                                            bool secondConst)
01168 {
01169 //      H_AUTO(PACS_PWI_reaction_long);
01170 
01171         // TODO: reaction for no collision must be made on the full deltaTime not only to CollisionTime
01172 
01173         // Get the two reaction codes
01174         UMovePrimitive::TReaction firstReaction=primitive.getReactionTypeInternal();
01175         UMovePrimitive::TReaction secondReaction=otherPrimitive.getReactionTypeInternal();
01176 
01177         // Overide collsion 
01178         collision = collision && (primitive.isObstacle ()) && (otherPrimitive.isObstacle ());
01179 
01180         // Get the two mass
01181         float mass0 = primitive.getMass ();
01182         float mass1 = otherPrimitive.getMass ();
01183 
01184         // Energy sum
01185         double projSpeed0 = desc.ContactNormal1 * _Speed;
01186         double projSpeed1 = desc.ContactNormal0 * second._Speed;
01187         double energySum = (- mass0 * projSpeed0 - mass1 * projSpeed1 ) / 2.0;
01188 
01189         // Old position
01190         CVectorD collisionPosition=_3dInitPosition;
01191         collisionPosition+=_Speed*desc.ContactTime;
01192 
01193         // Calc new speed
01194         CVectorD newSpeed;
01195         
01196         // Obstacle ?
01197         if ( (!collision) || (firstReaction==UMovePrimitive::DoNothing) )
01198                 newSpeed=_Speed;
01199         else
01200         {
01201                 switch (firstReaction)
01202                 {
01203                 case UMovePrimitive::Slide:
01204                         // Remove projected speed
01205                         newSpeed=_Speed - projSpeed0 * desc.ContactNormal1;
01206 
01207                         // Reflexion speed
01208                         newSpeed+=( primitive.getAttenuation()*energySum / mass0 ) * desc.ContactNormal1;
01209                         break;
01210                 case UMovePrimitive::Reflexion:
01211                         // Remove projected speed
01212                         newSpeed=_Speed - projSpeed0 * desc.ContactNormal1;
01213 
01214                         // Reflexion speed
01215                         newSpeed+=( primitive.getAttenuation()*energySum / mass0 ) * desc.ContactNormal1;
01216                         break;
01217                 case UMovePrimitive::Stop:
01218                         newSpeed.set (0,0,0);
01219                         break;
01220                 default: break;
01221                 }
01222         }
01223 
01224         // Set new speed
01225         setSpeed (newSpeed, container, &primitive, worldImage);
01226 
01227         // New position at t=0
01228         if (retriever)
01229         {
01230                 // Make a domove in the Ben data
01231                 double  deltaDist= _DeltaPosition.norm();
01232                 double  deltaTime;
01233                 if(deltaDist<0.000001)
01234                         deltaTime= 0;
01235                 else
01236                         deltaTime=(collisionPosition-_Position.getPos ()).norm()/deltaDist;
01237                 nlassert (deltaTime>=0);
01238                 nlassert (deltaTime<=1);
01239 
01240                 UGlobalPosition newPosition = retriever->doMove (_Position.getGlobalPos (), _DeltaPosition,
01241                         (float)deltaTime, surfaceTemp, true);
01242 
01243                 // Set the new position
01244                 _Position.setGlobalPos (newPosition, *retriever);
01245 
01246                 // Position at t=0
01247                 _3dInitPosition = _Position.getPos() - newSpeed * desc.ContactTime;
01248 
01249                 // New init time
01250                 _InitTime = desc.ContactTime;
01251         }
01252         else
01253         {
01254                 // No retriever used
01255                 _Position.setPos (collisionPosition);
01256 
01257                 // Position at t=0
01258                 _3dInitPosition = collisionPosition - newSpeed * desc.ContactTime;
01259 
01260                 // New init time
01261                 _InitTime = desc.ContactTime;
01262         }
01263 
01264         // Dirt pos
01265         dirtPos (container, &primitive, worldImage);
01266 
01267         // ****** Second object
01268         
01269         // Is second object in a static world ?
01270         if (!secondConst)
01271         {
01272                 // Old position
01273                 collisionPosition=second._3dInitPosition;
01274                 collisionPosition+=second._Speed * desc.ContactTime;
01275                 
01276                 // Obstacle ?
01277                 if ( (!collision) || (secondReaction==UMovePrimitive::DoNothing) )
01278                         newSpeed=second._Speed;
01279                 else
01280                 {
01281                         switch (secondReaction)
01282                         {
01283                         case UMovePrimitive::Slide:
01284                                 // Remove projected speed
01285                                 newSpeed=second._Speed - projSpeed1 * desc.ContactNormal0;
01286 
01287                                 // Reflexion speed
01288                                 newSpeed+=( otherPrimitive.getAttenuation()*energySum / mass1 ) * desc.ContactNormal1;
01289                                 break;
01290                         case UMovePrimitive::Reflexion:
01291                                 // Remove projected speed
01292                                 newSpeed=second._Speed - projSpeed1 * desc.ContactNormal0;
01293 
01294                                 // Reflexion speed
01295                                 newSpeed+=( otherPrimitive.getAttenuation()*energySum / mass1 ) * desc.ContactNormal0;
01296                                 break;
01297                         case UMovePrimitive::Stop:
01298                                 newSpeed.set (0,0,0);
01299                                 break;
01300                         default: break;
01301                         }
01302                 }
01303 
01304                 // Set new speed
01305                 second.setSpeed (newSpeed, container, &otherPrimitive, secondWorldImage);
01306 
01307                 // New position at t=0
01308                 if (retriever)
01309                 {
01310                         // Make a domove in the Ben data
01311                         double  deltaDist= second._DeltaPosition.norm();
01312                         double  deltaTime;
01313                         if(deltaDist==0)
01314                                 deltaTime= 0;
01315                         else
01316                                 deltaTime=(collisionPosition-second._Position.getPos ()).norm()/deltaDist;
01317                         clamp (deltaTime, 0.0, 1.0);
01318 
01319                         UGlobalPosition newPosition = retriever->doMove (second._Position.getGlobalPos (), second._DeltaPosition,
01320                                 (float)deltaTime, surfaceTemp, true);
01321 
01322                         // Set the new position
01323                         second._Position.setGlobalPos (newPosition, *retriever);
01324 
01325                         // Position at t=0
01326                         second._3dInitPosition = second._Position.getPos() - newSpeed * desc.ContactTime;
01327 
01328                         // New init time
01329                         second._InitTime = desc.ContactTime;
01330                 }
01331                 else
01332                 {
01333                         // No retriever used
01334                         second._Position.setPos (collisionPosition);
01335 
01336                         // Position at t=0
01337                         second._3dInitPosition = collisionPosition - newSpeed * desc.ContactTime;
01338 
01339                         // New init time
01340                         second._InitTime = desc.ContactTime;
01341                 }
01342 
01343                 // Dirt pos
01344                 second.dirtPos (container, &otherPrimitive, secondWorldImage);
01345         }
01346 }
01347 
01348 // ***************************************************************************
01349 
01350 void CPrimitiveWorldImage::reaction (const CCollisionSurfaceDesc&       surfaceDesc, const UGlobalPosition& globalPosition,
01351                                                            CGlobalRetriever& retriever, double ratio, double dt, CMovePrimitive &primitive, CMoveContainer &container,
01352                                                            uint8 worldImage)
01353 {
01354 //      H_AUTO(PACS_PWI_reaction_short);
01355 
01356         // Reaction type
01357         uint32 type=primitive.getReactionTypeInternal();
01358 
01359         // Reaction to the collision: copy the CGlobalRetriever::CGlobalPosition
01360         _Position.setGlobalPos (globalPosition, retriever);
01361         
01362         // Relfexion or slide ?
01363         if ((type==UMovePrimitive::Reflexion)||(type==UMovePrimitive::Slide))
01364         {
01365                 // Slide ?
01366                 if (type==UMovePrimitive::Slide)
01367                 {
01368                         // Project last delta on plane of collision.
01369                         _Speed-= surfaceDesc.ContactNormal*(surfaceDesc.ContactNormal*_Speed-NELPACS_DIST_BACK/(dt-surfaceDesc.ContactTime));
01370                 }
01371 
01372                 // Reflexion ?
01373                 if (type==UMovePrimitive::Reflexion)
01374                 {
01375                         // Project last delta on plane of collision.
01376                         double speedProj=surfaceDesc.ContactNormal*_Speed;
01377                         _Speed-=surfaceDesc.ContactNormal*(speedProj+speedProj*primitive.getAttenuation()-NELPACS_DIST_BACK/(dt-surfaceDesc.ContactTime));
01378                 }
01379         }
01380         else
01381         {
01382                 // Stop ?
01383                 if (type==UMovePrimitive::Stop)
01384                 {
01385                         _Speed.set (0,0,0);
01386                 }
01387         }
01388 
01389         // Contact time
01390         double contactTime=surfaceDesc.ContactTime;
01391 
01392         // Init position
01393         _3dInitPosition = _Position.getPos() - _Speed * contactTime;
01394 
01395         // Set contactTime
01396         _InitTime=contactTime;
01397 
01398         // Dirt pos
01399         dirtPos (&container, &primitive, worldImage);
01400 }
01401 
01402 // ***************************************************************************
01403 
01404 void CPrimitiveWorldImage::setGlobalPosition (const UGlobalPosition& pos, CMoveContainer& container, CMovePrimitive &primitive, uint8 worldImage)
01405 {
01406         // Cast type
01407         nlassert (dynamic_cast<const CMoveContainer*>(&container));
01408         const CMoveContainer *cont=(const CMoveContainer*)&container;
01409 
01410         // Use the global retriever ?
01411         nlassert (cont->getGlobalRetriever());
01412         
01413         // Get the pos
01414         _Position.setGlobalPos (pos, *cont->getGlobalRetriever());      
01415 
01416         // Precalc some values
01417         _3dInitPosition = _Position.getPos ();
01418         _InitTime = 0;
01419 
01420         // Speed NULL
01421         _Speed=CVector::Null;
01422 
01423         // Dirt BB
01424         dirtPos (&container, &primitive, worldImage);
01425 }
01426 
01427 // ***************************************************************************
01428 
01429 void CPrimitiveWorldImage::setGlobalPosition (const NLMISC::CVectorD& pos, CMoveContainer& container, CMovePrimitive &primitive, uint8 worldImage, bool keepZ /*= false*/)
01430 {
01431         // Cast type
01432         nlassert (dynamic_cast<const CMoveContainer*>(&container));
01433         const CMoveContainer *cont=(const CMoveContainer*)&container;
01434 
01435         // Get the retriever
01436         CGlobalRetriever *retriever=cont->getGlobalRetriever();
01437 
01438         // Use a global retriever
01439         if (retriever)
01440         {
01441                 // Get a cvector
01442 //              CVector vect=pos;               // better with CVectorD
01443 
01444                 // Get global position
01445                 UGlobalPosition globalPosition=retriever->retrievePosition (pos);
01446 
01447                 if (keepZ)
01448                 {
01449                         globalPosition.LocalPosition.Estimation.z = (float) pos.z;
01450                 }
01451                 // Set global position
01452                 _Position.setGlobalPos (globalPosition, *retriever);
01453         }
01454         else
01455         {
01456                 // Set the position
01457                 _Position.setPos (pos);
01458         }
01459 
01460         // Precalc some values
01461         _3dInitPosition = _Position.getPos ();
01462         _InitTime = 0;
01463 
01464         // Speed NULL
01465         _Speed=CVector::Null;
01466 
01467         // Dirt BB
01468         dirtPos (&container, &primitive, worldImage);
01469 }
01470 
01471 // ***************************************************************************
01472 
01473 void CPrimitiveWorldImage::move (const NLMISC::CVectorD& speed, CMoveContainer& container, CMovePrimitive &primitive, uint8 worldImage)
01474 {
01475         // New speed
01476         setSpeed (speed, &container, &primitive, worldImage);
01477 
01478         // Set initial position
01479         _3dInitPosition = _Position.getPos ();
01480 
01481         // Set initial time
01482         _InitTime = 0;
01483 
01484         // Dirt BB
01485         dirtPos (&container, &primitive, worldImage);
01486 }
01487 
01488 // ***************************************************************************
01489 
01490 
01491 } // NLPACS