00001
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
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
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
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
00069 this->operator=(source);
00070
00071
00072 _DynamicFlags&=~InModifiedListFlag;
00073
00074
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
00086
00087
00088 if (( (primitive.getCollisionMaskInternal() & otherPrimitive.getOcclusionMaskInternal()) == 0) &&
00089 ( (primitive.getOcclusionMaskInternal() & otherPrimitive.getCollisionMaskInternal()) == 0))
00090 return false;
00091
00092
00093 if ( (!primitive.checkTestTime (testTime, maxTestIteration)) || (!otherPrimitive.checkTestTime (testTime, maxTestIteration)) )
00094 return false;
00095
00096
00097 firstContactTime=FLT_MAX;
00098 lastContactTime=-FLT_MAX;
00099
00100
00101 switch (primitive.getPrimitiveTypeInternal())
00102 {
00103
00104
00105 case UMovePrimitive::_2DOrientedBox:
00106 {
00107
00108 switch (otherPrimitive.getPrimitiveTypeInternal())
00109 {
00110
00111
00112 case UMovePrimitive::_2DOrientedBox:
00113
00114 return evalCollisionOBoverOB (other, desc, timeMin, timeMax, firstContactTime, lastContactTime, primitive, otherPrimitive);
00115
00116
00117 case UMovePrimitive::_2DOrientedCylinder:
00118
00119 return evalCollisionOBoverOC (other, desc, timeMin, timeMax, firstContactTime, lastContactTime, primitive, otherPrimitive);
00120
00121 default:
00122
00123 nlstop;
00124 }
00125 }
00126
00127
00128 case UMovePrimitive::_2DOrientedCylinder:
00129 {
00130
00131 switch (otherPrimitive.getPrimitiveTypeInternal())
00132 {
00133
00134
00135 case UMovePrimitive::_2DOrientedBox:
00136 {
00137
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
00146 case UMovePrimitive::_2DOrientedCylinder:
00147
00148 return evalCollisionOCoverOC (other, desc, timeMin, timeMax, firstContactTime, lastContactTime, primitive, otherPrimitive);
00149
00150 default:
00151
00152 nlstop;
00153 }
00154 }
00155
00156 default:
00157
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
00170
00171
00172 if (!primitive.checkTestTime (testTime, maxTestIteration))
00173 return NULL;
00174
00175
00176 if (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox)
00177 {
00178
00179 CVector locI ((float)(_OBData.EdgeDirectionX[0]*primitive.getLength(0)/2.0), (float)(_OBData.EdgeDirectionY[0]*primitive.getLength(1)/2.0), 0);
00180
00181
00182 CVector locJ ((float)(_OBData.EdgeDirectionX[1]*primitive.getLength(0)/2.0), (float)(_OBData.EdgeDirectionY[1]*primitive.getLength(1)/2.0), 0);
00183
00184
00185 return retriever.testBBoxMove (_Position.getGlobalPos (), _DeltaPosition, locI, locJ, surfaceTemp);
00186 }
00187 else
00188 {
00189
00190 nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
00191
00192
00193
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 )
00202 {
00203
00204
00205
00206 double ratio;
00207 if (finalMax!=originalMax)
00208 ratio=(finalMax-_InitTime)/(originalMax-_InitTime);
00209 else
00210 ratio=1;
00211
00212
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
00224 _InitTime=finalMax;
00225 }
00226
00227
00228
00229 void CPrimitiveWorldImage::doMove (double timeMax)
00230 {
00231
00232
00233
00234 _Position.setPos (_Position.getPos ()+_Speed*(timeMax-_InitTime));
00235
00236
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
00247 nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
00248 nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
00249
00250
00251 bool find=false;
00252
00253
00254 desc.ContactTime=FLT_MAX;
00255
00256
00257 double _timeMax=-FLT_MAX;
00258
00259
00260 uint pt;
00261 uint seg;
00262 for (pt=0; pt<4; pt++)
00263 for (seg=0; seg<4; seg++)
00264 {
00265
00266 CCollisionDesc d;
00267 if ( evalCollisionPoverS (other, d, pt, seg, primitive, otherPrimitive) )
00268 {
00269
00270 find=true;
00271
00272
00273 if (d.ContactTime<desc.ContactTime)
00274 {
00275
00276 desc=d;
00277 }
00278
00279
00280 if (d.ContactTime>_timeMax)
00281 {
00282
00283 _timeMax=d.ContactTime;
00284 }
00285 }
00286 }
00287
00288
00289 for (pt=0; pt<4; pt++)
00290 for (seg=0; seg<4; seg++)
00291 {
00292
00293 CCollisionDesc d;
00294 if (other.evalCollisionPoverS (*this, d, pt, seg, primitive, otherPrimitive))
00295 {
00296
00297 find=true;
00298
00299
00300 if (d.ContactTime<desc.ContactTime)
00301 {
00302
00303 desc=d;
00304 }
00305
00306
00307 if (d.ContactTime>_timeMax)
00308 {
00309
00310 _timeMax=d.ContactTime;
00311 }
00312 }
00313 }
00314
00315 if (find)
00316 {
00317
00318 firstContactTime=desc.ContactTime;
00319 lastContactTime=_timeMax;
00320
00321
00322
00323
00324
00325
00326 if (timeMin > _timeMax)
00327
00328 return false;
00329
00330
00331 if (timeMax>desc.ContactTime)
00332 {
00333
00334 if (desc.ContactTime<timeMin)
00335 desc.ContactTime=timeMin;
00336
00337
00338 return true;
00339 }
00340 }
00341
00342
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
00353 nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
00354 nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
00355
00356
00357 bool find=false;
00358
00359
00360 desc.ContactTime=FLT_MAX;
00361
00362
00363 double _timeMax = -FLT_MAX;
00364
00365
00366 uint pt;
00367 for (pt=0; pt<4; pt++)
00368 {
00369
00370 CCollisionDesc d;
00371 double firstContactTime;
00372 double lastContactTime;
00373 if (evalCollisionPoverOC (other, d, pt, firstContactTime, lastContactTime, primitive, otherPrimitive))
00374 {
00375
00376 find=true;
00377
00378
00379 if (firstContactTime<desc.ContactTime)
00380 {
00381
00382 desc=d;
00383 }
00384
00385
00386 if (lastContactTime>_timeMax)
00387 {
00388
00389 _timeMax=lastContactTime;
00390 }
00391 }
00392 }
00393
00394
00395 uint seg;
00396 for (seg=0; seg<4; seg++)
00397 {
00398
00399 CCollisionDesc d;
00400 if (evalCollisionSoverOC (other, d, seg, primitive, otherPrimitive))
00401 {
00402
00403 find=true;
00404
00405
00406 if (d.ContactTime<desc.ContactTime)
00407 {
00408
00409 desc=d;
00410 }
00411
00412
00413 if (d.ContactTime>_timeMax)
00414 {
00415
00416 _timeMax=d.ContactTime;
00417 }
00418 }
00419 }
00420
00421 if (find)
00422 {
00423
00424 firstContactTime=desc.ContactTime;
00425 lastContactTime=_timeMax;
00426
00427
00428
00429
00430
00431
00432 if (timeMin > _timeMax)
00433
00434 return false;
00435
00436
00437 if (timeMax>desc.ContactTime)
00438 {
00439
00440 if (desc.ContactTime<timeMin)
00441 desc.ContactTime=timeMin;
00442
00443
00444 return true;
00445 }
00446 }
00447
00448
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
00458 nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
00459 nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
00460
00461
00462 const double normalSegX=other._OBData.EdgeDirectionY[numSeg];
00463 const double normalSegY=-other._OBData.EdgeDirectionX[numSeg];
00464
00465
00466 const double speedX=other._Speed.x-_Speed.x;
00467 const double speedY=other._Speed.y-_Speed.y;
00468
00469
00470 double dotProd= speedX*normalSegX + speedY*normalSegY;
00471
00472 if ( dotProd != 0 )
00473 {
00474
00475 double time= (normalSegX*(_OBData.PointPosX[numPoint] - other._OBData.PointPosX[numSeg]) +
00476 normalSegY*(_OBData.PointPosY[numPoint] - other._OBData.PointPosY[numSeg])) / dotProd;
00477
00478
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
00483 const double ptPosX= _OBData.PointPosX[numPoint] + _Speed.x*time;
00484 const double ptPosY= _OBData.PointPosY[numPoint] + _Speed.y*time;
00485
00486
00487 const double dirX= ptPosX - segPosX;
00488 const double dirY= ptPosY - segPosY;
00489
00490
00491 const double length= dirY*normalSegX - dirX*normalSegY;
00492
00493
00494 if ( ( length >= 0 ) && ( length <= otherPrimitive.getLength(numSeg&1) ) )
00495 {
00496
00497
00498
00499 const double pointSegZ=other._3dInitPosition.z;
00500 const double segPosZ= pointSegZ + other._Speed.z*time;
00501
00502
00503 const double pointZ=_3dInitPosition.z;
00504 const double ptPosZ= pointZ + _Speed.z*time;
00505
00506
00507 if ( (ptPosZ <= segPosZ + otherPrimitive.getHeightInternal()) && (ptPosZ + primitive.getHeightInternal() >= segPosZ) )
00508 {
00509
00510
00511
00512 desc.ContactTime=time;
00513
00514
00515 desc.ContactPosition.x=ptPosX;
00516 desc.ContactPosition.y=ptPosY;
00517 desc.ContactPosition.z=std::max (segPosZ, ptPosZ);
00518
00519
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
00528 return true;
00529 }
00530 }
00531 }
00532
00533
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
00545 d=(double)sqrt (d);
00546
00547
00548 a=0.5f/a;
00549
00550
00551 s0 = (-b-d)*a;
00552 s1 = (-b+d)*a;
00553
00554 return 2;
00555 }
00556 else if (d<0)
00557 {
00558
00559 return 0;
00560 }
00561 else
00562 {
00563
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
00577 nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
00578 nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
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
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
00615 double time;
00616
00617
00618 if (numSolution==1)
00619 {
00620 firstContactTime=s0;
00621 lastContactTime=s0;
00622 }
00623 else
00624 {
00625
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
00640 const double pointCylZ=other._3dInitPosition.z;
00641 const double cylPosZ= pointCylZ + other._Speed.z*time;
00642
00643
00644 const double pointZ=_3dInitPosition.z;
00645 const double ptPosZ= pointZ + _Speed.z*time;
00646
00647
00648 if ( (ptPosZ <= cylPosZ + otherPrimitive.getHeightInternal()) && (ptPosZ + primitive.getHeightInternal() >= cylPosZ) )
00649 {
00650
00651
00652
00653 desc.ContactTime=time;
00654
00655
00656 const double ptPosX= _OBData.PointPosX[numPoint] + _Speed.x*time;
00657 const double ptPosY= _OBData.PointPosY[numPoint] + _Speed.y*time;
00658
00659
00660 const double cylPosX= other._3dInitPosition.x + other._Speed.x*time;
00661 const double cylPosY= other._3dInitPosition.y + other._Speed.y*time;
00662
00663
00664 desc.ContactPosition.x=ptPosX;
00665 desc.ContactPosition.y=ptPosY;
00666 desc.ContactPosition.z=std::max (cylPosZ, ptPosZ);
00667
00668
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
00678 return true;
00679 }
00680 }
00681
00682
00683 return false;
00684 }
00685
00686
00687
00688 bool CPrimitiveWorldImage::evalCollisionSoverOC (CPrimitiveWorldImage& other, CCollisionDesc& desc, uint numSeg, CMovePrimitive& primitive,
00689 CMovePrimitive& otherPrimitive)
00690 {
00691
00692 nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
00693 nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
00694
00695
00696 const double normalSegX=_OBData.EdgeDirectionY[numSeg];
00697 const double normalSegY=-_OBData.EdgeDirectionX[numSeg];
00698
00699
00700 const double speedX=other._Speed.x-_Speed.x;
00701 const double speedY=other._Speed.y-_Speed.y;
00702
00703
00704 double dotProd= speedX*normalSegX + speedY*normalSegY;
00705
00706 if ( dotProd !=0 )
00707 {
00708
00709 double time= (otherPrimitive.getRadiusInternal() + normalSegX*(_OBData.PointPosX[numSeg] - other._3dInitPosition.x ) +
00710 normalSegY*(_OBData.PointPosY[numSeg] - other._3dInitPosition.y ) ) / dotProd;
00711
00712
00713 const double segPosX= _OBData.PointPosX[numSeg] + _Speed.x*time;
00714 const double segPosY= _OBData.PointPosY[numSeg] + _Speed.y*time;
00715
00716
00717 const double cylPosX= other._3dInitPosition.x + _Speed.x*time;
00718 const double cylPosY= other._3dInitPosition.y + _Speed.y*time;
00719
00720
00721 const double contactX= cylPosX - normalSegX*otherPrimitive.getRadiusInternal();
00722 const double contactY= cylPosY - normalSegY*otherPrimitive.getRadiusInternal();
00723
00724
00725 const double dirX= contactX - segPosX;
00726 const double dirY= contactY - segPosY;
00727
00728
00729 const double length= dirY*normalSegX - dirX*normalSegY;
00730
00731
00732 if ( ( length >= 0 ) && ( length <= primitive.getLength (numSeg&1) ) )
00733 {
00734
00735
00736
00737 const double segPosZ= _3dInitPosition.z + _Speed.z*time;
00738
00739
00740 const double cylPosZ= other._3dInitPosition.z + other._Speed.z*time;
00741
00742
00743 if ( (cylPosZ <= segPosZ + primitive.getHeightInternal() ) && (cylPosZ + otherPrimitive.getHeightInternal() >= segPosZ) )
00744 {
00745
00746
00747
00748 desc.ContactTime=time;
00749
00750
00751 desc.ContactPosition.x=contactX;
00752 desc.ContactPosition.y=contactY;
00753 desc.ContactPosition.z=std::max (segPosZ, cylPosZ);
00754
00755
00756 desc.ContactNormal0.x=normalSegX;
00757 desc.ContactNormal0.y=normalSegY;
00758 desc.ContactNormal0.z=0;
00759
00760
00761 desc.ContactNormal1.x=contactX-cylPosX;
00762 desc.ContactNormal1.y=contactY-cylPosY;
00763 desc.ContactNormal1.z=0;
00764 desc.ContactNormal1.normalize ();
00765
00766
00767 return true;
00768 }
00769 }
00770 }
00771
00772
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
00784 nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
00785 nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
00786
00787
00788
00789
00790
00791
00792
00793
00794
00795
00796
00797
00798
00799
00800
00801
00802
00803
00804
00805
00806
00807
00808
00809
00810
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
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
00824 double _timeMin, _timeMax;
00825
00826
00827 if (numSolution==1)
00828 {
00829 _timeMin=s0;
00830 _timeMax=s0;
00831 }
00832 else
00833 {
00834
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
00848
00849
00850
00851 firstContactTime=_timeMin;
00852 lastContactTime=_timeMax;
00853
00854
00855 if ((timeMin<_timeMax)&&(_timeMin<timeMax))
00856 {
00857
00858 const double cyl0Time= _timeMin;
00859 const double pointCyl0Z=_3dInitPosition.z;
00860 const double cyl0PosZ= pointCyl0Z + _Speed.z*cyl0Time;
00861
00862
00863 const double cyl1Time= _timeMin;
00864 const double pointCyl1Z=other._3dInitPosition.z;
00865 const double cyl1PosZ= pointCyl1Z + other._Speed.z * cyl1Time;
00866
00867
00868 if ( (cyl0PosZ <= cyl1PosZ + otherPrimitive.getHeightInternal() ) && (cyl0PosZ + primitive.getHeightInternal() >= cyl1PosZ) )
00869 {
00870
00871
00872
00873 desc.ContactTime=std::max (_timeMin, timeMin);
00874
00875
00876 const double cyl0PosX= _3dInitPosition.x + _Speed.x*cyl0Time;
00877 const double cyl0PosY= _3dInitPosition.y + _Speed.y*cyl0Time;
00878
00879
00880 const double cyl1PosX= other._3dInitPosition.x + other._Speed.x*cyl1Time;
00881 const double cyl1PosY= other._3dInitPosition.y + other._Speed.y*cyl1Time;
00882
00883
00884 desc.ContactNormal0.x= cyl1PosX - cyl0PosX;
00885 desc.ContactNormal0.y= cyl1PosY - cyl0PosY;
00886 desc.ContactNormal0.z= 0;
00887 desc.ContactNormal0.normalize ();
00888
00889
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
00895 desc.ContactNormal1.x= -desc.ContactNormal0.x;
00896 desc.ContactNormal1.y= -desc.ContactNormal0.y;
00897 desc.ContactNormal1.z= 0;
00898
00899
00900 return true;
00901 }
00902 }
00903 }
00904
00905
00906 return false;
00907 }
00908
00909
00910
00911 void CPrimitiveWorldImage::precalcPos (CMovePrimitive &primitive)
00912 {
00913
00914 uint type=primitive.getPrimitiveTypeInternal();
00915
00916
00917 if (type==UMovePrimitive::_2DOrientedBox)
00918 {
00919
00920 double cosinus=(double)cos(_OBData.Orientation);
00921 double sinus=(double)sin(_OBData.Orientation);
00922
00923
00924 double halfWidth=primitive.getLength (0)/2;
00925 double halfDepth=primitive.getLength (1)/2;
00926
00927
00928 _OBData.PointPosX[0]=cosinus*(-halfWidth)-sinus*(-halfDepth)+_3dInitPosition.x;
00929 _OBData.PointPosY[0]=sinus*(-halfWidth)+cosinus*(-halfDepth)+_3dInitPosition.y;
00930
00931
00932 _OBData.PointPosX[1]=cosinus*halfWidth-sinus*(-halfDepth)+_3dInitPosition.x;
00933 _OBData.PointPosY[1]=sinus*halfWidth+cosinus*(-halfDepth)+_3dInitPosition.y;
00934
00935
00936 _OBData.PointPosX[2]=cosinus*halfWidth-sinus*halfDepth+_3dInitPosition.x;
00937 _OBData.PointPosY[2]=sinus*halfWidth+cosinus*halfDepth+_3dInitPosition.y;
00938
00939
00940 _OBData.PointPosX[3]=cosinus*(-halfWidth)-sinus*halfDepth+_3dInitPosition.x;
00941 _OBData.PointPosY[3]=sinus*(-halfWidth)+cosinus*halfDepth+_3dInitPosition.y;
00942
00943
00944 double oneOverLength[2]= { 1 / primitive.getLength(0), 1 / primitive.getLength(1) };
00945
00946
00947 uint i;
00948 for (i=0; i<4; i++)
00949 {
00950
00951 uint next=(i+1)&3;
00952 double oneOver=oneOverLength[i&1];
00953
00954
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
00962 nlassert (type==UMovePrimitive::_2DOrientedCylinder);
00963 }
00964 }
00965
00966
00967
00968 void CPrimitiveWorldImage::precalcBB (double beginTime, double endTime, CMovePrimitive &primitive)
00969 {
00970
00971 uint type=primitive.getPrimitiveTypeInternal();
00972
00973
00974 if (type==UMovePrimitive::_2DOrientedBox)
00975 {
00976
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
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
01007
01008
01009
01010
01011
01012
01013
01014
01015
01016
01017
01018 }
01019 else
01020 {
01021
01022 nlassert (type==UMovePrimitive::_2DOrientedCylinder);
01023
01024
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
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
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
01059 uint slot;
01060 for (slot=0; slot<4; slot++)
01061 {
01062
01063 if (_MoveElement[slot]==NULL)
01064 {
01065
01066 double cx=(_BBXMin+_BBXMax)/2.f;
01067
01068
01069 _MoveElement[slot]=container.allocateMoveElement ();
01070 _MoveElement[slot]->Primitive=primitive;
01071 _MoveElement[slot]->X=x;
01072 _MoveElement[slot]->Y=y;
01073
01074
01075 if (cx<centerX)
01076
01077 cell.linkFirstX (_MoveElement[slot]);
01078 else
01079
01080 cell.linkLastX (_MoveElement[slot]);
01081
01082
01083
01084
01085
01086
01087
01088
01089
01090
01091 cell.updateSortedLists (_MoveElement[slot], worldImage);
01092
01093
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
01105 uint slot;
01106 for (slot=0; slot<4; slot++)
01107 {
01108
01109 if (_MoveElement[slot]==NULL)
01110 {
01111
01112 _MoveElement[slot]=container.allocateMoveElement ();
01113 _MoveElement[slot]->Primitive=primitive;
01114 _MoveElement[slot]->X=x;
01115 _MoveElement[slot]->Y=y;
01116
01117
01118 cell.linkLastX (_MoveElement[slot]);
01119
01120
01121 break;
01122 }
01123 }
01124 }
01125
01126
01127
01128 void CPrimitiveWorldImage::removeMoveElement (uint i, CMoveContainer &container, uint8 worldImage)
01129 {
01130
01131 nlassert ((i>=0)||(i<4));
01132 nlassert (_MoveElement[i]!=NULL);
01133
01134
01135 container.unlinkMoveElement (_MoveElement[i], worldImage);
01136
01137
01138 container.freeMoveElement (_MoveElement[i]);
01139
01140
01141 _MoveElement[i]=NULL;
01142 }
01143
01144
01145
01146 void CPrimitiveWorldImage::checkSortedList (uint8 worldImage)
01147 {
01148
01149 for (uint i=0; i<4; i++)
01150 {
01151
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
01170
01171
01172
01173
01174 UMovePrimitive::TReaction firstReaction=primitive.getReactionTypeInternal();
01175 UMovePrimitive::TReaction secondReaction=otherPrimitive.getReactionTypeInternal();
01176
01177
01178 collision = collision && (primitive.isObstacle ()) && (otherPrimitive.isObstacle ());
01179
01180
01181 float mass0 = primitive.getMass ();
01182 float mass1 = otherPrimitive.getMass ();
01183
01184
01185 double projSpeed0 = desc.ContactNormal1 * _Speed;
01186 double projSpeed1 = desc.ContactNormal0 * second._Speed;
01187 double energySum = (- mass0 * projSpeed0 - mass1 * projSpeed1 ) / 2.0;
01188
01189
01190 CVectorD collisionPosition=_3dInitPosition;
01191 collisionPosition+=_Speed*desc.ContactTime;
01192
01193
01194 CVectorD newSpeed;
01195
01196
01197 if ( (!collision) || (firstReaction==UMovePrimitive::DoNothing) )
01198 newSpeed=_Speed;
01199 else
01200 {
01201 switch (firstReaction)
01202 {
01203 case UMovePrimitive::Slide:
01204
01205 newSpeed=_Speed - projSpeed0 * desc.ContactNormal1;
01206
01207
01208 newSpeed+=( primitive.getAttenuation()*energySum / mass0 ) * desc.ContactNormal1;
01209 break;
01210 case UMovePrimitive::Reflexion:
01211
01212 newSpeed=_Speed - projSpeed0 * desc.ContactNormal1;
01213
01214
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
01225 setSpeed (newSpeed, container, &primitive, worldImage);
01226
01227
01228 if (retriever)
01229 {
01230
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
01244 _Position.setGlobalPos (newPosition, *retriever);
01245
01246
01247 _3dInitPosition = _Position.getPos() - newSpeed * desc.ContactTime;
01248
01249
01250 _InitTime = desc.ContactTime;
01251 }
01252 else
01253 {
01254
01255 _Position.setPos (collisionPosition);
01256
01257
01258 _3dInitPosition = collisionPosition - newSpeed * desc.ContactTime;
01259
01260
01261 _InitTime = desc.ContactTime;
01262 }
01263
01264
01265 dirtPos (container, &primitive, worldImage);
01266
01267
01268
01269
01270 if (!secondConst)
01271 {
01272
01273 collisionPosition=second._3dInitPosition;
01274 collisionPosition+=second._Speed * desc.ContactTime;
01275
01276
01277 if ( (!collision) || (secondReaction==UMovePrimitive::DoNothing) )
01278 newSpeed=second._Speed;
01279 else
01280 {
01281 switch (secondReaction)
01282 {
01283 case UMovePrimitive::Slide:
01284
01285 newSpeed=second._Speed - projSpeed1 * desc.ContactNormal0;
01286
01287
01288 newSpeed+=( otherPrimitive.getAttenuation()*energySum / mass1 ) * desc.ContactNormal1;
01289 break;
01290 case UMovePrimitive::Reflexion:
01291
01292 newSpeed=second._Speed - projSpeed1 * desc.ContactNormal0;
01293
01294
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
01305 second.setSpeed (newSpeed, container, &otherPrimitive, secondWorldImage);
01306
01307
01308 if (retriever)
01309 {
01310
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
01323 second._Position.setGlobalPos (newPosition, *retriever);
01324
01325
01326 second._3dInitPosition = second._Position.getPos() - newSpeed * desc.ContactTime;
01327
01328
01329 second._InitTime = desc.ContactTime;
01330 }
01331 else
01332 {
01333
01334 second._Position.setPos (collisionPosition);
01335
01336
01337 second._3dInitPosition = collisionPosition - newSpeed * desc.ContactTime;
01338
01339
01340 second._InitTime = desc.ContactTime;
01341 }
01342
01343
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
01355
01356
01357 uint32 type=primitive.getReactionTypeInternal();
01358
01359
01360 _Position.setGlobalPos (globalPosition, retriever);
01361
01362
01363 if ((type==UMovePrimitive::Reflexion)||(type==UMovePrimitive::Slide))
01364 {
01365
01366 if (type==UMovePrimitive::Slide)
01367 {
01368
01369 _Speed-= surfaceDesc.ContactNormal*(surfaceDesc.ContactNormal*_Speed-NELPACS_DIST_BACK/(dt-surfaceDesc.ContactTime));
01370 }
01371
01372
01373 if (type==UMovePrimitive::Reflexion)
01374 {
01375
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
01383 if (type==UMovePrimitive::Stop)
01384 {
01385 _Speed.set (0,0,0);
01386 }
01387 }
01388
01389
01390 double contactTime=surfaceDesc.ContactTime;
01391
01392
01393 _3dInitPosition = _Position.getPos() - _Speed * contactTime;
01394
01395
01396 _InitTime=contactTime;
01397
01398
01399 dirtPos (&container, &primitive, worldImage);
01400 }
01401
01402
01403
01404 void CPrimitiveWorldImage::setGlobalPosition (const UGlobalPosition& pos, CMoveContainer& container, CMovePrimitive &primitive, uint8 worldImage)
01405 {
01406
01407 nlassert (dynamic_cast<const CMoveContainer*>(&container));
01408 const CMoveContainer *cont=(const CMoveContainer*)&container;
01409
01410
01411 nlassert (cont->getGlobalRetriever());
01412
01413
01414 _Position.setGlobalPos (pos, *cont->getGlobalRetriever());
01415
01416
01417 _3dInitPosition = _Position.getPos ();
01418 _InitTime = 0;
01419
01420
01421 _Speed=CVector::Null;
01422
01423
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 )
01430 {
01431
01432 nlassert (dynamic_cast<const CMoveContainer*>(&container));
01433 const CMoveContainer *cont=(const CMoveContainer*)&container;
01434
01435
01436 CGlobalRetriever *retriever=cont->getGlobalRetriever();
01437
01438
01439 if (retriever)
01440 {
01441
01442
01443
01444
01445 UGlobalPosition globalPosition=retriever->retrievePosition (pos);
01446
01447 if (keepZ)
01448 {
01449 globalPosition.LocalPosition.Estimation.z = (float) pos.z;
01450 }
01451
01452 _Position.setGlobalPos (globalPosition, *retriever);
01453 }
01454 else
01455 {
01456
01457 _Position.setPos (pos);
01458 }
01459
01460
01461 _3dInitPosition = _Position.getPos ();
01462 _InitTime = 0;
01463
01464
01465 _Speed=CVector::Null;
01466
01467
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
01476 setSpeed (speed, &container, &primitive, worldImage);
01477
01478
01479 _3dInitPosition = _Position.getPos ();
01480
01481
01482 _InitTime = 0;
01483
01484
01485 dirtPos (&container, &primitive, worldImage);
01486 }
01487
01488
01489
01490
01491 }