mirror of
https://port.numenaute.org/aleajactaest/khanat-opennel-code.git
synced 2024-09-20 19:50:32 +00:00
1490 lines
41 KiB
C++
1490 lines
41 KiB
C++
// NeL - MMORPG Framework <http://dev.ryzom.com/projects/nel/>
|
|
// Copyright (C) 2010 Winch Gate Property Limited
|
|
//
|
|
// This program is free software: you can redistribute it and/or modify
|
|
// it under the terms of the GNU Affero General Public License as
|
|
// published by the Free Software Foundation, either version 3 of the
|
|
// License, or (at your option) any later version.
|
|
//
|
|
// This program is distributed in the hope that it will be useful,
|
|
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
// GNU Affero General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU Affero General Public License
|
|
// along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
#include "stdpacs.h"
|
|
|
|
#include "nel/misc/hierarchical_timer.h"
|
|
|
|
#include "primitive_world_image.h"
|
|
#include "move_primitive.h"
|
|
#include "move_element.h"
|
|
|
|
using namespace NLMISC;
|
|
|
|
|
|
namespace NLPACS
|
|
{
|
|
|
|
// ***************************************************************************
|
|
|
|
CPrimitiveWorldImage::CPrimitiveWorldImage()
|
|
{
|
|
// Set to NULL
|
|
for (uint i=0; i<4; i++)
|
|
_MoveElement[i]=NULL;
|
|
|
|
_DynamicFlags=0;
|
|
_BBXMin=-FLT_MAX;
|
|
_BBXMax=-FLT_MAX;
|
|
_BBYMin=-FLT_MAX;
|
|
_BBYMax=-FLT_MAX;
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
void CPrimitiveWorldImage::deleteIt (CMoveContainer &container, uint8 worldImage)
|
|
{
|
|
// Free the move elements
|
|
for (uint i=0; i<4; i++)
|
|
if (_MoveElement[i])
|
|
removeMoveElement (i, container, worldImage);
|
|
}
|
|
// ***************************************************************************
|
|
|
|
void CPrimitiveWorldImage::copy (const CPrimitiveWorldImage& source)
|
|
{
|
|
// Copy
|
|
this->operator=(source);
|
|
|
|
// Reset some flags
|
|
_DynamicFlags&=~InModifiedListFlag;
|
|
|
|
// Pointer into the 4 possibles sorted lists of movable primitives. Must be NULL
|
|
for (uint i=0; i<4; i++)
|
|
_MoveElement[i]=NULL;
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
bool CPrimitiveWorldImage::evalCollision (CPrimitiveWorldImage& other, CCollisionDesc& desc, double timeMin, double timeMax, uint32 testTime,
|
|
uint32 maxTestIteration, double &firstContactTime, double &lastContactTime, CMovePrimitive& primitive,
|
|
CMovePrimitive& otherPrimitive)
|
|
{
|
|
// H_AUTO(PACS_PWI_evalCollision_long);
|
|
|
|
// Mask test
|
|
if (( (primitive.getCollisionMaskInternal() & otherPrimitive.getOcclusionMaskInternal()) == 0) &&
|
|
( (primitive.getOcclusionMaskInternal() & otherPrimitive.getCollisionMaskInternal()) == 0))
|
|
return false;
|
|
|
|
// Test time
|
|
if ( (!primitive.checkTestTime (testTime, maxTestIteration)) || (!otherPrimitive.checkTestTime (testTime, maxTestIteration)) )
|
|
return false;
|
|
|
|
// Clear time min time max
|
|
firstContactTime=FLT_MAX;
|
|
lastContactTime=-FLT_MAX;
|
|
|
|
// Switch the good test
|
|
switch (primitive.getPrimitiveTypeInternal())
|
|
{
|
|
|
|
// Static box over...
|
|
case UMovePrimitive::_2DOrientedBox:
|
|
{
|
|
// Switch second type
|
|
switch (otherPrimitive.getPrimitiveTypeInternal())
|
|
{
|
|
|
|
// Static box over movable box
|
|
case UMovePrimitive::_2DOrientedBox:
|
|
// Make the test
|
|
return evalCollisionOBoverOB (other, desc, timeMin, timeMax, firstContactTime, lastContactTime, primitive, otherPrimitive);
|
|
|
|
// Static box over movable cylinder
|
|
case UMovePrimitive::_2DOrientedCylinder:
|
|
// Make the test
|
|
return evalCollisionOBoverOC (other, desc, timeMin, timeMax, firstContactTime, lastContactTime, primitive, otherPrimitive);
|
|
|
|
default:
|
|
// Should not go here
|
|
nlstop;
|
|
}
|
|
}
|
|
|
|
// Static box over...
|
|
case UMovePrimitive::_2DOrientedCylinder:
|
|
{
|
|
// Switch second type
|
|
switch (otherPrimitive.getPrimitiveTypeInternal())
|
|
{
|
|
|
|
// Static box over movable box
|
|
case UMovePrimitive::_2DOrientedBox:
|
|
{
|
|
// Make the test
|
|
bool collid=other.evalCollisionOBoverOC (*this, desc, timeMin, timeMax, firstContactTime, lastContactTime, otherPrimitive,
|
|
primitive);
|
|
if (collid)
|
|
desc.XChgContactNormals ();
|
|
return collid;
|
|
}
|
|
|
|
// Static box over movable cylinder
|
|
case UMovePrimitive::_2DOrientedCylinder:
|
|
// Make the test
|
|
return evalCollisionOCoverOC (other, desc, timeMin, timeMax, firstContactTime, lastContactTime, primitive, otherPrimitive);
|
|
|
|
default:
|
|
// Should not go here
|
|
nlstop;
|
|
}
|
|
}
|
|
|
|
default:
|
|
// Should not go here
|
|
nlstop;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
const TCollisionSurfaceDescVector *CPrimitiveWorldImage::evalCollision (CGlobalRetriever &retriever, CCollisionSurfaceTemp& surfaceTemp,
|
|
uint32 testTime, uint32 maxTestIteration, CMovePrimitive& primitive)
|
|
{
|
|
// H_AUTO(PACS_PWI_evalCollision_short);
|
|
|
|
// Test time
|
|
if (!primitive.checkTestTime (testTime, maxTestIteration))
|
|
return NULL;
|
|
|
|
// Switch the good test
|
|
if (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox)
|
|
{
|
|
// Local I
|
|
CVector locI ((float)(_OBData.EdgeDirectionX[0]*primitive.getLength(0)/2.0), (float)(_OBData.EdgeDirectionY[0]*primitive.getLength(1)/2.0), 0);
|
|
|
|
// Local J
|
|
CVector locJ ((float)(_OBData.EdgeDirectionX[1]*primitive.getLength(0)/2.0), (float)(_OBData.EdgeDirectionY[1]*primitive.getLength(1)/2.0), 0);
|
|
|
|
// Test
|
|
return retriever.testBBoxMove (_Position.getGlobalPos (), _DeltaPosition, locI, locJ, surfaceTemp);
|
|
}
|
|
else
|
|
{
|
|
// Check
|
|
nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
|
|
|
|
// Test
|
|
//nlinfo ("1) %f %f %f\n", _DeltaPosition.x, _DeltaPosition.y, _DeltaPosition.z);
|
|
|
|
return retriever.testCylinderMove (_Position.getGlobalPos (), _DeltaPosition, primitive.getRadiusInternal(), surfaceTemp);
|
|
}
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
void CPrimitiveWorldImage::doMove (CGlobalRetriever &retriever, CCollisionSurfaceTemp& surfaceTemp, double originalMax, double finalMax, bool keepZ /*= false*/)
|
|
{
|
|
H_AUTO(NLPACS_PWI_Do_Move);
|
|
|
|
|
|
// Time to avance
|
|
double ratio;
|
|
if (finalMax!=originalMax)
|
|
ratio=(finalMax-_InitTime)/(originalMax-_InitTime);
|
|
else
|
|
ratio=1;
|
|
|
|
// Make the move
|
|
if (!keepZ)
|
|
{
|
|
_Position.setGlobalPos (retriever.doMove(_Position.getGlobalPos(), _DeltaPosition, (float)ratio, surfaceTemp, false), retriever);
|
|
}
|
|
else
|
|
{
|
|
_Position.setGlobalPosKeepZ(retriever.doMove(_Position.getGlobalPos(), _DeltaPosition, (float)ratio, surfaceTemp, false), retriever);
|
|
}
|
|
|
|
|
|
// Final position
|
|
_InitTime=finalMax;
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
void CPrimitiveWorldImage::doMove (double timeMax)
|
|
{
|
|
// H_AUTO(PACS_PWI_doMove_short);
|
|
|
|
// Make the move
|
|
_Position.setPos (_Position.getPos ()+_Speed*(timeMax-_InitTime));
|
|
|
|
// Final position
|
|
_InitTime=timeMax;
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
bool CPrimitiveWorldImage::evalCollisionOBoverOB (CPrimitiveWorldImage& other, CCollisionDesc& desc, double timeMin, double timeMax,
|
|
double &firstContactTime, double &lastContactTime, CMovePrimitive& primitive,
|
|
CMovePrimitive& otherPrimitive)
|
|
{
|
|
// Checks
|
|
nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
|
|
nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
|
|
|
|
// Find a collision
|
|
bool find=false;
|
|
|
|
// Best time
|
|
desc.ContactTime=FLT_MAX;
|
|
|
|
// Timemin
|
|
double _timeMax=-FLT_MAX;
|
|
|
|
// Check movable points over the edge
|
|
uint pt;
|
|
uint seg;
|
|
for (pt=0; pt<4; pt++)
|
|
for (seg=0; seg<4; seg++)
|
|
{
|
|
// Get collision time of the point over the segment
|
|
CCollisionDesc d;
|
|
if ( evalCollisionPoverS (other, d, pt, seg, primitive, otherPrimitive) )
|
|
{
|
|
// Find
|
|
find=true;
|
|
|
|
// Best time ?
|
|
if (d.ContactTime<desc.ContactTime)
|
|
{
|
|
// This is the new descriptor
|
|
desc=d;
|
|
}
|
|
|
|
// Best max time ?
|
|
if (d.ContactTime>_timeMax)
|
|
{
|
|
// This is the new max time
|
|
_timeMax=d.ContactTime;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Check static points over the movable box
|
|
for (pt=0; pt<4; pt++)
|
|
for (seg=0; seg<4; seg++)
|
|
{
|
|
// Get collision time of the point over the segment
|
|
CCollisionDesc d;
|
|
if (other.evalCollisionPoverS (*this, d, pt, seg, primitive, otherPrimitive))
|
|
{
|
|
// Find
|
|
find=true;
|
|
|
|
// Best time ?
|
|
if (d.ContactTime<desc.ContactTime)
|
|
{
|
|
// This is the new descriptor
|
|
desc=d;
|
|
}
|
|
|
|
// Best max time ?
|
|
if (d.ContactTime>_timeMax)
|
|
{
|
|
// This is the new max time
|
|
_timeMax=d.ContactTime;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (find)
|
|
{
|
|
// First last contact time
|
|
firstContactTime=desc.ContactTime;
|
|
lastContactTime=_timeMax;
|
|
|
|
// Half time
|
|
//double halfTime = (_timeMax+desc.ContactTime)/2.0;
|
|
|
|
// Collision in the past ?
|
|
//if (timeMin > halfTime)
|
|
if (timeMin > _timeMax)
|
|
// yes, abort
|
|
return false;
|
|
|
|
// Collision not in the future ?
|
|
if (timeMax>desc.ContactTime)
|
|
{
|
|
// Clamp time
|
|
if (desc.ContactTime<timeMin)
|
|
desc.ContactTime=timeMin;
|
|
|
|
// yes, found it
|
|
return true;
|
|
}
|
|
}
|
|
|
|
// No collision found
|
|
return false;
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
bool CPrimitiveWorldImage::evalCollisionOBoverOC (CPrimitiveWorldImage& other, CCollisionDesc& desc, double timeMin, double timeMax,
|
|
double &firstContactTime, double &lastContactTime, CMovePrimitive& primitive,
|
|
CMovePrimitive& otherPrimitive)
|
|
{
|
|
// Checks
|
|
nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
|
|
nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
|
|
|
|
// Find a collision
|
|
bool find=false;
|
|
|
|
// Best time
|
|
desc.ContactTime=FLT_MAX;
|
|
|
|
// time min clip
|
|
double _timeMax = -FLT_MAX;
|
|
|
|
// Check movable points over the cylinder
|
|
uint pt;
|
|
for (pt=0; pt<4; pt++)
|
|
{
|
|
// Get collision time of the point over the segment
|
|
CCollisionDesc d;
|
|
double firstContactTime;
|
|
double lastContactTime;
|
|
if (evalCollisionPoverOC (other, d, pt, firstContactTime, lastContactTime, primitive, otherPrimitive))
|
|
{
|
|
// Found
|
|
find=true;
|
|
|
|
// Best time ?
|
|
if (firstContactTime<desc.ContactTime)
|
|
{
|
|
// This is the new descriptor
|
|
desc=d;
|
|
}
|
|
|
|
// Best max time ?
|
|
if (lastContactTime>_timeMax)
|
|
{
|
|
// New max time
|
|
_timeMax=lastContactTime;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Check static points over the movable box
|
|
uint seg;
|
|
for (seg=0; seg<4; seg++)
|
|
{
|
|
// Get collision time of the point over the segment
|
|
CCollisionDesc d;
|
|
if (evalCollisionSoverOC (other, d, seg, primitive, otherPrimitive))
|
|
{
|
|
// Found
|
|
find=true;
|
|
|
|
// Best time ?
|
|
if (d.ContactTime<desc.ContactTime)
|
|
{
|
|
// This is the new descriptor
|
|
desc=d;
|
|
}
|
|
|
|
// Best max time ?
|
|
if (d.ContactTime>_timeMax)
|
|
{
|
|
// New max time
|
|
_timeMax=d.ContactTime;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (find)
|
|
{
|
|
// First last contact time
|
|
firstContactTime=desc.ContactTime;
|
|
lastContactTime=_timeMax;
|
|
|
|
// Half time
|
|
//double halfTime = (_timeMax+desc.ContactTime)/2.0;
|
|
|
|
// Collision in the past ?
|
|
//if (timeMin > halfTime)
|
|
if (timeMin > _timeMax)
|
|
// yes, abort
|
|
return false;
|
|
|
|
// Collision not in the future ?
|
|
if (timeMax>desc.ContactTime)
|
|
{
|
|
// Clamp time
|
|
if (desc.ContactTime<timeMin)
|
|
desc.ContactTime=timeMin;
|
|
|
|
// yes, found it
|
|
return true;
|
|
}
|
|
}
|
|
|
|
// No collision found
|
|
return false;
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
bool CPrimitiveWorldImage::evalCollisionPoverS (CPrimitiveWorldImage& other, CCollisionDesc& desc, uint numPoint, uint numSeg,
|
|
CMovePrimitive& primitive, CMovePrimitive& otherPrimitive)
|
|
{
|
|
// Checks
|
|
nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
|
|
nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
|
|
|
|
// Some constants
|
|
const double normalSegX=other._OBData.EdgeDirectionY[numSeg];
|
|
const double normalSegY=-other._OBData.EdgeDirectionX[numSeg];
|
|
|
|
// Relative speed
|
|
const double speedX=other._Speed.x-_Speed.x;
|
|
const double speedY=other._Speed.y-_Speed.y;
|
|
|
|
// Dot product with the plan tangeante
|
|
double dotProd= speedX*normalSegX + speedY*normalSegY;
|
|
//if ( dotProd > 0 )
|
|
if ( dotProd != 0 )
|
|
{
|
|
// Time of the collision
|
|
double time= (normalSegX*(_OBData.PointPosX[numPoint] - other._OBData.PointPosX[numSeg]) +
|
|
normalSegY*(_OBData.PointPosY[numPoint] - other._OBData.PointPosY[numSeg])) / dotProd;
|
|
|
|
// Position of segment point at collision time
|
|
const double segPosX= other._OBData.PointPosX[numSeg] + other._Speed.x*time;
|
|
const double segPosY= other._OBData.PointPosY[numSeg] + other._Speed.y*time;
|
|
|
|
// Position of the point at collision time
|
|
const double ptPosX= _OBData.PointPosX[numPoint] + _Speed.x*time;
|
|
const double ptPosY= _OBData.PointPosY[numPoint] + _Speed.y*time;
|
|
|
|
// Direction of the collision on the segment
|
|
const double dirX= ptPosX - segPosX;
|
|
const double dirY= ptPosY - segPosY;
|
|
|
|
// Length of this vector
|
|
const double length= dirY*normalSegX - dirX*normalSegY;
|
|
|
|
// Included ?
|
|
if ( ( length >= 0 ) && ( length <= otherPrimitive.getLength(numSeg&1) ) )
|
|
{
|
|
// 2d Collid checked... Now check height
|
|
|
|
// Pos Z
|
|
const double pointSegZ=other._3dInitPosition.z;
|
|
const double segPosZ= pointSegZ + other._Speed.z*time;
|
|
|
|
// Some constants
|
|
const double pointZ=_3dInitPosition.z;
|
|
const double ptPosZ= pointZ + _Speed.z*time;
|
|
|
|
// Included ?
|
|
if ( (ptPosZ <= segPosZ + otherPrimitive.getHeightInternal()) && (ptPosZ + primitive.getHeightInternal() >= segPosZ) )
|
|
{
|
|
// Ok Collision, fill the result
|
|
|
|
// Time
|
|
desc.ContactTime=time;
|
|
|
|
// Position
|
|
desc.ContactPosition.x=ptPosX;
|
|
desc.ContactPosition.y=ptPosY;
|
|
desc.ContactPosition.z=std::max (segPosZ, ptPosZ);
|
|
|
|
// Seg box normal
|
|
desc.ContactNormal1.x=normalSegX;
|
|
desc.ContactNormal1.y=normalSegY;
|
|
desc.ContactNormal1.z=0;
|
|
desc.ContactNormal0.x=-desc.ContactNormal1.x;
|
|
desc.ContactNormal0.y=-desc.ContactNormal1.y;
|
|
desc.ContactNormal0.z=0;
|
|
|
|
// End
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
|
|
// No collision
|
|
return false;
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
inline uint secondDegree (double a, double b, double c, double& s0, double& s1)
|
|
{
|
|
double d=b*b-4.f*a*c;
|
|
if (d>0)
|
|
{
|
|
// sqrt d
|
|
d=(double)sqrt (d);
|
|
|
|
// 1 / 2a
|
|
a=0.5f/a;
|
|
|
|
// 2 solutions
|
|
s0 = (-b-d)*a;
|
|
s1 = (-b+d)*a;
|
|
|
|
return 2;
|
|
}
|
|
else if (d<0)
|
|
{
|
|
// No solution
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
// 1 solution
|
|
s0 = -b/(2.f*a);
|
|
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
bool CPrimitiveWorldImage::evalCollisionPoverOC (CPrimitiveWorldImage& other, CCollisionDesc& desc, uint numPoint,
|
|
double &firstContactTime, double &lastContactTime, CMovePrimitive& primitive,
|
|
CMovePrimitive& otherPrimitive)
|
|
{
|
|
// Checks
|
|
nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
|
|
nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
|
|
|
|
/* Point Equ:
|
|
* p(t) = p0 + v0*(t - t0)
|
|
*
|
|
* Cylinder center Equ:
|
|
* p'(t) = p'0 + v'0*(t - t'0)
|
|
*
|
|
* Find t for this equation:
|
|
* R^2 = Norm^2 (p(t) - p'(t))
|
|
* R^2 = Norm^2 ( p0 + v0 ( t - t0 ) - p'0 - v'0 ( t - t'0 ) )
|
|
*
|
|
* A = p0 - v0*t0 - p'0 + v'0*t'0
|
|
* B = (v0 - v'0)
|
|
*
|
|
* Norm^2 (B)*t^2 + 2*(A.B)*t + Norm^2 (A) - R^2 = 0
|
|
*
|
|
* a = Norm^2 (B)
|
|
* b = 2*(A.B)
|
|
* c = Norm^2 (A) - R^2
|
|
*
|
|
* a*t^2 + b*t + c = 0
|
|
*/
|
|
|
|
// Let's go
|
|
const double _Ax = _OBData.PointPosX[numPoint] - other._3dInitPosition.x;
|
|
const double _Ay = _OBData.PointPosY[numPoint] - other._3dInitPosition.y;
|
|
const double _Bx = _Speed.x - other._Speed.x;
|
|
const double _By = _Speed.y - other._Speed.y;
|
|
|
|
// Eval system
|
|
double s0, s1;
|
|
double squareRadius=otherPrimitive.getRadiusInternal()*otherPrimitive.getRadiusInternal();
|
|
uint numSolution=secondDegree (_Bx*_Bx+_By*_By, 2.f*(_Ax*_Bx+_Ay*_By), _Ax*_Ax+_Ay*_Ay-squareRadius, s0, s1);
|
|
if (numSolution!=0)
|
|
{
|
|
// time
|
|
double time;
|
|
|
|
// Collision time
|
|
if (numSolution==1)
|
|
{
|
|
firstContactTime=s0;
|
|
lastContactTime=s0;
|
|
}
|
|
else
|
|
{
|
|
// First and last time
|
|
if (s0<s1)
|
|
{
|
|
firstContactTime=s0;
|
|
lastContactTime=s1;
|
|
}
|
|
else
|
|
{
|
|
firstContactTime=s1;
|
|
lastContactTime=s0;
|
|
}
|
|
}
|
|
time=firstContactTime;
|
|
|
|
// Pos Z
|
|
const double pointCylZ=other._3dInitPosition.z;
|
|
const double cylPosZ= pointCylZ + other._Speed.z*time;
|
|
|
|
// Some constants
|
|
const double pointZ=_3dInitPosition.z;
|
|
const double ptPosZ= pointZ + _Speed.z*time;
|
|
|
|
// Z Included ?
|
|
if ( (ptPosZ <= cylPosZ + otherPrimitive.getHeightInternal()) && (ptPosZ + primitive.getHeightInternal() >= cylPosZ) )
|
|
{
|
|
// Ok Collision, fill the result
|
|
|
|
// Time
|
|
desc.ContactTime=time;
|
|
|
|
// Point position
|
|
const double ptPosX= _OBData.PointPosX[numPoint] + _Speed.x*time;
|
|
const double ptPosY= _OBData.PointPosY[numPoint] + _Speed.y*time;
|
|
|
|
// Cylinder position
|
|
const double cylPosX= other._3dInitPosition.x + other._Speed.x*time;
|
|
const double cylPosY= other._3dInitPosition.y + other._Speed.y*time;
|
|
|
|
// Position
|
|
desc.ContactPosition.x=ptPosX;
|
|
desc.ContactPosition.y=ptPosY;
|
|
desc.ContactPosition.z=std::max (cylPosZ, ptPosZ);
|
|
|
|
// Cylinder normal
|
|
desc.ContactNormal1.x=ptPosX-cylPosX;
|
|
desc.ContactNormal1.y=ptPosY-cylPosY;
|
|
desc.ContactNormal1.z=0;
|
|
desc.ContactNormal1.normalize ();
|
|
desc.ContactNormal0.x=-desc.ContactNormal1.x;
|
|
desc.ContactNormal0.y=-desc.ContactNormal1.y;
|
|
desc.ContactNormal0.z=0;
|
|
|
|
// End
|
|
return true;
|
|
}
|
|
}
|
|
|
|
// No collision
|
|
return false;
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
bool CPrimitiveWorldImage::evalCollisionSoverOC (CPrimitiveWorldImage& other, CCollisionDesc& desc, uint numSeg, CMovePrimitive& primitive,
|
|
CMovePrimitive& otherPrimitive)
|
|
{
|
|
// Checks
|
|
nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedBox);
|
|
nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
|
|
|
|
// Some constants
|
|
const double normalSegX=_OBData.EdgeDirectionY[numSeg];
|
|
const double normalSegY=-_OBData.EdgeDirectionX[numSeg];
|
|
|
|
// Relative speed
|
|
const double speedX=other._Speed.x-_Speed.x;
|
|
const double speedY=other._Speed.y-_Speed.y;
|
|
|
|
// Dot product with the plan tangeante
|
|
double dotProd= speedX*normalSegX + speedY*normalSegY;
|
|
//if ( dotProd < 0 )
|
|
if ( dotProd !=0 )
|
|
{
|
|
// Time of the collision
|
|
double time= (otherPrimitive.getRadiusInternal() + normalSegX*(_OBData.PointPosX[numSeg] - other._3dInitPosition.x ) +
|
|
normalSegY*(_OBData.PointPosY[numSeg] - other._3dInitPosition.y ) ) / dotProd;
|
|
|
|
// Position of segment point at collision time
|
|
const double segPosX= _OBData.PointPosX[numSeg] + _Speed.x*time;
|
|
const double segPosY= _OBData.PointPosY[numSeg] + _Speed.y*time;
|
|
|
|
// Position of the cylinder at collision time
|
|
const double cylPosX= other._3dInitPosition.x + _Speed.x*time;
|
|
const double cylPosY= other._3dInitPosition.y + _Speed.y*time;
|
|
|
|
// Position de contact
|
|
const double contactX= cylPosX - normalSegX*otherPrimitive.getRadiusInternal();
|
|
const double contactY= cylPosY - normalSegY*otherPrimitive.getRadiusInternal();
|
|
|
|
// Direction of the collision on the segment
|
|
const double dirX= contactX - segPosX;
|
|
const double dirY= contactY - segPosY;
|
|
|
|
// Length of this vector
|
|
const double length= dirY*normalSegX - dirX*normalSegY;
|
|
|
|
// Included ?
|
|
if ( ( length >= 0 ) && ( length <= primitive.getLength (numSeg&1) ) )
|
|
{
|
|
// 2d Collid checked... Now check height
|
|
|
|
// Pos Z
|
|
const double segPosZ= _3dInitPosition.z + _Speed.z*time;
|
|
|
|
// Some constants
|
|
const double cylPosZ= other._3dInitPosition.z + other._Speed.z*time;
|
|
|
|
// Included ?
|
|
if ( (cylPosZ <= segPosZ + primitive.getHeightInternal() ) && (cylPosZ + otherPrimitive.getHeightInternal() >= segPosZ) )
|
|
{
|
|
// Ok Collision, fill the result
|
|
|
|
// Time
|
|
desc.ContactTime=time;
|
|
|
|
// Position
|
|
desc.ContactPosition.x=contactX;
|
|
desc.ContactPosition.y=contactY;
|
|
desc.ContactPosition.z=std::max (segPosZ, cylPosZ);
|
|
|
|
// Segment normal
|
|
desc.ContactNormal0.x=normalSegX;
|
|
desc.ContactNormal0.y=normalSegY;
|
|
desc.ContactNormal0.z=0;
|
|
|
|
// Seg box normal
|
|
desc.ContactNormal1.x=contactX-cylPosX;
|
|
desc.ContactNormal1.y=contactY-cylPosY;
|
|
desc.ContactNormal1.z=0;
|
|
desc.ContactNormal1.normalize ();
|
|
|
|
// End
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
|
|
// No collision
|
|
return false;
|
|
}
|
|
|
|
|
|
// ***************************************************************************
|
|
|
|
bool CPrimitiveWorldImage::evalCollisionOCoverOC (CPrimitiveWorldImage& other, CCollisionDesc& desc, double timeMin, double timeMax,
|
|
double &firstContactTime, double &lastContactTime, CMovePrimitive& primitive,
|
|
CMovePrimitive& otherPrimitive)
|
|
{
|
|
// Checks
|
|
nlassert (primitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
|
|
nlassert (otherPrimitive.getPrimitiveTypeInternal()==UMovePrimitive::_2DOrientedCylinder);
|
|
|
|
|
|
/* Cylinder0 center equ:
|
|
* p(t) = p0 + v0*(t - t0)
|
|
*
|
|
* Cylinder1 center equ:
|
|
* p'(t) = p'0 + v'0*(t - t'0)
|
|
*
|
|
* Find t for this equation:
|
|
* (R + R')^2 = Norm^2 (p(t) - p'(t))
|
|
* (R + R')^2 = Norm^2 ( p0 + v0 ( t - t0 ) - p'0 - v'0 ( t - t'0 ) )
|
|
*
|
|
* A = p0 - v0*t0 - p'0 + v'0*t'0
|
|
* B = (v0 - v'0)
|
|
*
|
|
* Norm^2 (B)*t^2 + 2*(A.B)*t + Norm^2 (A) - (R + R')^2 = 0
|
|
*
|
|
* a = Norm^2 (B)
|
|
* b = 2*(A.B)
|
|
* c = Norm^2 (A) - (R + R')^2
|
|
*
|
|
* a*t^2 + b*t + c = 0
|
|
*/
|
|
|
|
// Let's go
|
|
const double _Ax = _3dInitPosition.x - other._3dInitPosition.x;
|
|
const double _Ay = _3dInitPosition.y - other._3dInitPosition.y;
|
|
const double _Bx = _Speed.x - other._Speed.x;
|
|
const double _By = _Speed.y - other._Speed.y;
|
|
|
|
// Eval system
|
|
double s0, s1;
|
|
double radiusSquare=primitive.getRadiusInternal()+otherPrimitive.getRadiusInternal();
|
|
radiusSquare*=radiusSquare;
|
|
uint numSolution=secondDegree (_Bx*_Bx+_By*_By, 2.f*(_Ax*_Bx+_Ay*_By), _Ax*_Ax+_Ay*_Ay-radiusSquare, s0, s1);
|
|
if (numSolution!=0)
|
|
{
|
|
// time
|
|
double _timeMin, _timeMax;
|
|
|
|
// Collision time
|
|
if (numSolution==1)
|
|
{
|
|
_timeMin=s0;
|
|
_timeMax=s0;
|
|
}
|
|
else
|
|
{
|
|
// Time min and max
|
|
if (s0>s1)
|
|
{
|
|
_timeMin=s1;
|
|
_timeMax=s0;
|
|
}
|
|
else
|
|
{
|
|
_timeMin=s0;
|
|
_timeMax=s1;
|
|
}
|
|
}
|
|
|
|
// half time
|
|
//const double halfTime=(_timeMin+_timeMax)/2.0;
|
|
|
|
// Conatct time
|
|
firstContactTime=_timeMin;
|
|
lastContactTime=_timeMax;
|
|
|
|
// Clip time
|
|
if ((timeMin<_timeMax)&&(_timeMin<timeMax))
|
|
{
|
|
// Some constants
|
|
const double cyl0Time= _timeMin;
|
|
const double pointCyl0Z=_3dInitPosition.z;
|
|
const double cyl0PosZ= pointCyl0Z + _Speed.z*cyl0Time;
|
|
|
|
// Pos Z
|
|
const double cyl1Time= _timeMin;
|
|
const double pointCyl1Z=other._3dInitPosition.z;
|
|
const double cyl1PosZ= pointCyl1Z + other._Speed.z * cyl1Time;
|
|
|
|
// Z Included ?
|
|
if ( (cyl0PosZ <= cyl1PosZ + otherPrimitive.getHeightInternal() ) && (cyl0PosZ + primitive.getHeightInternal() >= cyl1PosZ) )
|
|
{
|
|
// Ok Collision, fill the result
|
|
|
|
// Time
|
|
desc.ContactTime=std::max (_timeMin, timeMin);
|
|
|
|
// Cylinder 0 position
|
|
const double cyl0PosX= _3dInitPosition.x + _Speed.x*cyl0Time;
|
|
const double cyl0PosY= _3dInitPosition.y + _Speed.y*cyl0Time;
|
|
|
|
// Cylinder 1 position
|
|
const double cyl1PosX= other._3dInitPosition.x + other._Speed.x*cyl1Time;
|
|
const double cyl1PosY= other._3dInitPosition.y + other._Speed.y*cyl1Time;
|
|
|
|
// First cylinder normal
|
|
desc.ContactNormal0.x= cyl1PosX - cyl0PosX;
|
|
desc.ContactNormal0.y= cyl1PosY - cyl0PosY;
|
|
desc.ContactNormal0.z= 0;
|
|
desc.ContactNormal0.normalize ();
|
|
|
|
// Contact position
|
|
desc.ContactPosition.x= desc.ContactNormal0.x*primitive.getRadiusInternal() + cyl0PosX;
|
|
desc.ContactPosition.y= desc.ContactNormal0.y*primitive.getRadiusInternal() + cyl0PosY;
|
|
desc.ContactPosition.z= std::max (cyl0PosZ, cyl1PosZ);
|
|
|
|
// Second cylinder normal
|
|
desc.ContactNormal1.x= -desc.ContactNormal0.x;
|
|
desc.ContactNormal1.y= -desc.ContactNormal0.y;
|
|
desc.ContactNormal1.z= 0;
|
|
|
|
// End
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
|
|
// No collision
|
|
return false;
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
void CPrimitiveWorldImage::precalcPos (CMovePrimitive &primitive)
|
|
{
|
|
// Type of the primitive
|
|
uint type=primitive.getPrimitiveTypeInternal();
|
|
|
|
// Box ?
|
|
if (type==UMovePrimitive::_2DOrientedBox)
|
|
{
|
|
// Calc cosinus and sinus
|
|
double cosinus=(double)cos(_OBData.Orientation);
|
|
double sinus=(double)sin(_OBData.Orientation);
|
|
|
|
// Size
|
|
double halfWidth=primitive.getLength (0)/2;
|
|
double halfDepth=primitive.getLength (1)/2;
|
|
|
|
// First point
|
|
_OBData.PointPosX[0]=cosinus*(-halfWidth)-sinus*(-halfDepth)+_3dInitPosition.x;
|
|
_OBData.PointPosY[0]=sinus*(-halfWidth)+cosinus*(-halfDepth)+_3dInitPosition.y;
|
|
|
|
// Second point
|
|
_OBData.PointPosX[1]=cosinus*halfWidth-sinus*(-halfDepth)+_3dInitPosition.x;
|
|
_OBData.PointPosY[1]=sinus*halfWidth+cosinus*(-halfDepth)+_3dInitPosition.y;
|
|
|
|
// Third point
|
|
_OBData.PointPosX[2]=cosinus*halfWidth-sinus*halfDepth+_3dInitPosition.x;
|
|
_OBData.PointPosY[2]=sinus*halfWidth+cosinus*halfDepth+_3dInitPosition.y;
|
|
|
|
// Fourth point
|
|
_OBData.PointPosX[3]=cosinus*(-halfWidth)-sinus*halfDepth+_3dInitPosition.x;
|
|
_OBData.PointPosY[3]=sinus*(-halfWidth)+cosinus*halfDepth+_3dInitPosition.y;
|
|
|
|
// Direction
|
|
double length0 = (primitive.getLength(0)==0)? 0.001 : primitive.getLength(0);
|
|
double length1 = (primitive.getLength(1)==0)? 0.001 : primitive.getLength(1);
|
|
double oneOverLength[2]= { 1 / length0, 1 / length1 };
|
|
|
|
// Direction
|
|
uint i;
|
|
for (i=0; i<4; i++)
|
|
{
|
|
// Next index
|
|
uint next=(i+1)&3;
|
|
double oneOver=oneOverLength[i&1];
|
|
|
|
// New direction
|
|
_OBData.EdgeDirectionX[i]=(_OBData.PointPosX[next] - _OBData.PointPosX[i])*oneOver;
|
|
_OBData.EdgeDirectionY[i]=(_OBData.PointPosY[next] - _OBData.PointPosY[i])*oneOver;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Should be a cylinder
|
|
nlassert (type==UMovePrimitive::_2DOrientedCylinder);
|
|
}
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
void CPrimitiveWorldImage::precalcBB (double beginTime, double endTime, CMovePrimitive &primitive)
|
|
{
|
|
// Type of the primitive
|
|
uint type=primitive.getPrimitiveTypeInternal();
|
|
|
|
// Box ?
|
|
if (type==UMovePrimitive::_2DOrientedBox)
|
|
{
|
|
// Orientation index
|
|
sint orient= (sint)(256.f*_OBData.Orientation/(2.f*NLMISC::Pi));
|
|
orient&=0xff;
|
|
orient>>=6;
|
|
nlassert (orient>=0);
|
|
nlassert (orient<4);
|
|
|
|
// Compute coordinates
|
|
_BBXMin=FLT_MAX;
|
|
_BBYMin=FLT_MAX;
|
|
_BBXMax=-FLT_MAX;
|
|
_BBYMax=-FLT_MAX;
|
|
|
|
for (uint i=0; i<4; i++)
|
|
{
|
|
if (_OBData.PointPosX[i]<_BBXMin)
|
|
_BBXMin=_OBData.PointPosX[i];
|
|
if (_OBData.PointPosX[i]>_BBXMax)
|
|
_BBXMax=_OBData.PointPosX[i];
|
|
if (_OBData.PointPosY[i]<_BBYMin)
|
|
_BBYMin=_OBData.PointPosY[i];
|
|
if (_OBData.PointPosY[i]>_BBYMax)
|
|
_BBYMax=_OBData.PointPosY[i];
|
|
}
|
|
_BBXMin=std::min (std::min (_BBXMin, _BBXMin+endTime*_Speed.x), _BBXMin+beginTime*_Speed.x);
|
|
_BBXMax=std::max (std::max (_BBXMax, _BBXMax+endTime*_Speed.x), _BBXMax+beginTime*_Speed.x);
|
|
_BBYMin=std::min (std::min (_BBYMin, _BBYMin+endTime*_Speed.y), _BBYMin+beginTime*_Speed.y);
|
|
_BBYMax=std::max (std::max (_BBYMax, _BBYMax+endTime*_Speed.y), _BBYMax+beginTime*_Speed.y);
|
|
|
|
/*
|
|
// This code is faster but buggy..
|
|
_BBXMin= _OBData.PointPosX[minX[orient]] + _Speed.x*beginTime;
|
|
_BBXMin= std::min (_BBXMin, _OBData.PointPosX[minX[orient]] + _Speed.x*endTime);
|
|
|
|
_BBYMin= _OBData.PointPosY[minY[orient]] + _Speed.y*beginTime;
|
|
_BBYMin= std::min (_BBYMin, _OBData.PointPosY[minY[orient]] + _Speed.y*endTime);
|
|
|
|
_BBXMax= _OBData.PointPosX[maxX[orient]] + _Speed.x*beginTime;
|
|
_BBXMax= std::max (_BBXMax, _OBData.PointPosX[maxX[orient]] + _Speed.x*endTime);
|
|
|
|
_BBYMax= _OBData.PointPosY[maxY[orient]] + _Speed.y*beginTime;
|
|
_BBYMax= std::max (_BBYMax, _OBData.PointPosY[maxY[orient]] + _Speed.y*endTime);*/
|
|
}
|
|
else
|
|
{
|
|
// Should be a cylinder
|
|
nlassert (type==UMovePrimitive::_2DOrientedCylinder);
|
|
|
|
// Compute X coordinates
|
|
_BBXMin= _3dInitPosition.x + _Speed.x*beginTime;
|
|
_BBXMax= _3dInitPosition.x + _Speed.x*endTime;
|
|
if (_BBXMin>_BBXMax)
|
|
{
|
|
double tmp=_BBXMin;
|
|
_BBXMin=_BBXMax;
|
|
_BBXMax=tmp;
|
|
}
|
|
_BBXMin-=primitive.getRadiusInternal();
|
|
_BBXMax+=primitive.getRadiusInternal();
|
|
|
|
// Compute Y coordinates
|
|
_BBYMin= _3dInitPosition.y + _Speed.y*beginTime;
|
|
_BBYMax= _3dInitPosition.y + _Speed.y*endTime;
|
|
if (_BBYMin>_BBYMax)
|
|
{
|
|
double tmp=_BBYMin;
|
|
_BBYMin=_BBYMax;
|
|
_BBYMax=tmp;
|
|
}
|
|
_BBYMin-=primitive.getRadiusInternal();
|
|
_BBYMax+=primitive.getRadiusInternal();
|
|
}
|
|
|
|
// Delta position
|
|
_DeltaPosition=_Speed*(endTime-beginTime);
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
void CPrimitiveWorldImage::addMoveElement (CMoveCell& cell, uint16 x, uint16 y, double centerX, double /* centerY */, CMovePrimitive *primitive,
|
|
CMoveContainer &container, uint8 worldImage)
|
|
{
|
|
// Find a free place
|
|
uint slot;
|
|
for (slot=0; slot<4; slot++)
|
|
{
|
|
// Empty ?
|
|
if (_MoveElement[slot]==NULL)
|
|
{
|
|
// Primitive center
|
|
double cx=(_BBXMin+_BBXMax)/2.f;
|
|
|
|
// Allocate move element
|
|
_MoveElement[slot]=container.allocateMoveElement ();
|
|
_MoveElement[slot]->Primitive=primitive;
|
|
_MoveElement[slot]->X=x;
|
|
_MoveElement[slot]->Y=y;
|
|
|
|
// Insert in left or right ?
|
|
if (cx<centerX)
|
|
// In the left
|
|
cell.linkFirstX (_MoveElement[slot]);
|
|
else
|
|
// In the right
|
|
cell.linkLastX (_MoveElement[slot]);
|
|
|
|
/*// Insert in left or right ?
|
|
if (cy<centerY)
|
|
// In the left
|
|
cell.linkFirstY (_MoveElement[slot]);
|
|
else
|
|
// In the right
|
|
cell.linkLastY (_MoveElement[slot]);*/
|
|
|
|
// Move it
|
|
cell.updateSortedLists (_MoveElement[slot], worldImage);
|
|
|
|
// End
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
void CPrimitiveWorldImage::addMoveElementendOfList (CMoveCell& cell, uint16 x, uint16 y, CMovePrimitive *primitive,
|
|
CMoveContainer &container)
|
|
{
|
|
// Find a free place
|
|
uint slot;
|
|
for (slot=0; slot<4; slot++)
|
|
{
|
|
// Empty ?
|
|
if (_MoveElement[slot]==NULL)
|
|
{
|
|
// Allocate move element
|
|
_MoveElement[slot]=container.allocateMoveElement ();
|
|
_MoveElement[slot]->Primitive=primitive;
|
|
_MoveElement[slot]->X=x;
|
|
_MoveElement[slot]->Y=y;
|
|
|
|
// In the right
|
|
cell.linkLastX (_MoveElement[slot]);
|
|
|
|
// End
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
void CPrimitiveWorldImage::removeMoveElement (uint i, CMoveContainer &container, uint8 worldImage)
|
|
{
|
|
// Check
|
|
nlassert (i<4);
|
|
nlassert (_MoveElement[i]!=NULL);
|
|
|
|
// Unlink the element
|
|
container.unlinkMoveElement (_MoveElement[i], worldImage);
|
|
|
|
// Free the move element
|
|
container.freeMoveElement (_MoveElement[i]);
|
|
|
|
// Set to NULL
|
|
_MoveElement[i]=NULL;
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
void CPrimitiveWorldImage::checkSortedList (uint8 worldImage)
|
|
{
|
|
// For the 4 elements
|
|
for (uint i=0; i<4; i++)
|
|
{
|
|
// element here ?
|
|
if (_MoveElement[i])
|
|
{
|
|
if (_MoveElement[i]->PreviousX)
|
|
nlassertonce (_MoveElement[i]->PreviousX->Primitive->getWorldImage(worldImage)->_BBXMin <= _BBXMin);
|
|
if (_MoveElement[i]->NextX)
|
|
nlassertonce (_BBXMin <= _MoveElement[i]->NextX->Primitive->getWorldImage(worldImage)->_BBXMin);
|
|
}
|
|
}
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
void CPrimitiveWorldImage::reaction (CPrimitiveWorldImage& second, const CCollisionDesc& desc, CGlobalRetriever* retriever,
|
|
CCollisionSurfaceTemp& surfaceTemp, bool collision, CMovePrimitive &primitive,
|
|
CMovePrimitive &otherPrimitive, CMoveContainer *container, uint8 worldImage, uint8 secondWorldImage,
|
|
bool secondConst)
|
|
{
|
|
// H_AUTO(PACS_PWI_reaction_long);
|
|
|
|
// Get the two reaction codes
|
|
UMovePrimitive::TReaction firstReaction=primitive.getReactionTypeInternal();
|
|
UMovePrimitive::TReaction secondReaction=otherPrimitive.getReactionTypeInternal();
|
|
|
|
// Overide collsion
|
|
collision = collision && (primitive.isObstacle ()) && (otherPrimitive.isObstacle ());
|
|
|
|
// Get the two mass
|
|
float mass0 = primitive.getMass ();
|
|
float mass1 = otherPrimitive.getMass ();
|
|
|
|
// Energy sum
|
|
double projSpeed0 = desc.ContactNormal1 * _Speed;
|
|
double projSpeed1 = desc.ContactNormal0 * second._Speed;
|
|
double energySum = (- mass0 * projSpeed0 - mass1 * projSpeed1 ) / 2.0;
|
|
|
|
// Old position
|
|
CVectorD collisionPosition=_3dInitPosition;
|
|
collisionPosition+=_Speed*desc.ContactTime;
|
|
|
|
// Calc new speed
|
|
CVectorD newSpeed(0.0, 0.0, 0.0);
|
|
|
|
// Obstacle ?
|
|
if (collision)
|
|
{
|
|
switch (firstReaction)
|
|
{
|
|
case UMovePrimitive::Slide:
|
|
// Remove projected speed
|
|
newSpeed=_Speed - projSpeed0 * desc.ContactNormal1;
|
|
|
|
// Reflexion speed
|
|
newSpeed+=( primitive.getAttenuation()*energySum / mass0 ) * desc.ContactNormal1;
|
|
break;
|
|
case UMovePrimitive::Reflexion:
|
|
// Remove projected speed
|
|
newSpeed=_Speed - projSpeed0 * desc.ContactNormal1;
|
|
|
|
// Reflexion speed
|
|
newSpeed+=( primitive.getAttenuation()*energySum / mass0 ) * desc.ContactNormal1;
|
|
break;
|
|
case UMovePrimitive::Stop:
|
|
newSpeed.set (0,0,0);
|
|
break;
|
|
case UMovePrimitive::DoNothing:
|
|
newSpeed=_Speed;
|
|
break;
|
|
default: break;
|
|
}
|
|
|
|
// Set new speed
|
|
setSpeed (newSpeed, container, &primitive, worldImage);
|
|
|
|
// New position at t=0
|
|
if (retriever)
|
|
{
|
|
// Make a domove in the Ben data
|
|
double deltaDist= _DeltaPosition.norm();
|
|
double deltaTime;
|
|
if(deltaDist<0.000001)
|
|
deltaTime= 0;
|
|
else
|
|
deltaTime=(collisionPosition-_Position.getPos ()).norm()/deltaDist;
|
|
nlassert (deltaTime>=0);
|
|
nlassert (deltaTime<=1);
|
|
|
|
UGlobalPosition newPosition = retriever->doMove (_Position.getGlobalPos (), _DeltaPosition,
|
|
(float)deltaTime, surfaceTemp, true);
|
|
|
|
// Set the new position
|
|
_Position.setGlobalPos (newPosition, *retriever);
|
|
|
|
// Position at t=0
|
|
_3dInitPosition = _Position.getPos() - newSpeed * desc.ContactTime;
|
|
|
|
// New init time
|
|
_InitTime = desc.ContactTime;
|
|
}
|
|
else
|
|
{
|
|
// No retriever used
|
|
_Position.setPos (collisionPosition);
|
|
|
|
// Position at t=0
|
|
_3dInitPosition = collisionPosition - newSpeed * desc.ContactTime;
|
|
|
|
// New init time
|
|
_InitTime = desc.ContactTime;
|
|
}
|
|
|
|
// Dirt pos
|
|
dirtPos (container, &primitive, worldImage);
|
|
|
|
// ****** Second object
|
|
|
|
// Is second object in a static world ?
|
|
if (!secondConst)
|
|
{
|
|
// Old position
|
|
collisionPosition=second._3dInitPosition;
|
|
collisionPosition+=second._Speed * desc.ContactTime;
|
|
|
|
// Obstacle ?
|
|
switch (secondReaction)
|
|
{
|
|
case UMovePrimitive::Slide:
|
|
// Remove projected speed
|
|
newSpeed=second._Speed - projSpeed1 * desc.ContactNormal0;
|
|
|
|
// Reflexion speed
|
|
newSpeed+=( otherPrimitive.getAttenuation()*energySum / mass1 ) * desc.ContactNormal1;
|
|
break;
|
|
case UMovePrimitive::Reflexion:
|
|
// Remove projected speed
|
|
newSpeed=second._Speed - projSpeed1 * desc.ContactNormal0;
|
|
|
|
// Reflexion speed
|
|
newSpeed+=( otherPrimitive.getAttenuation()*energySum / mass1 ) * desc.ContactNormal0;
|
|
break;
|
|
case UMovePrimitive::Stop:
|
|
newSpeed.set (0,0,0);
|
|
break;
|
|
case UMovePrimitive::DoNothing:
|
|
newSpeed=second._Speed;
|
|
break;
|
|
default: break;
|
|
}
|
|
|
|
// Set new speed
|
|
second.setSpeed (newSpeed, container, &otherPrimitive, secondWorldImage);
|
|
|
|
// New position at t=0
|
|
if (retriever)
|
|
{
|
|
// Make a domove in the Ben data
|
|
double deltaDist= second._DeltaPosition.norm();
|
|
double deltaTime;
|
|
if(deltaDist==0)
|
|
deltaTime= 0;
|
|
else
|
|
deltaTime=(collisionPosition-second._Position.getPos ()).norm()/deltaDist;
|
|
clamp (deltaTime, 0.0, 1.0);
|
|
|
|
UGlobalPosition newPosition = retriever->doMove (second._Position.getGlobalPos (), second._DeltaPosition,
|
|
(float)deltaTime, surfaceTemp, true);
|
|
|
|
// Set the new position
|
|
second._Position.setGlobalPos (newPosition, *retriever);
|
|
|
|
// Position at t=0
|
|
second._3dInitPosition = second._Position.getPos() - newSpeed * desc.ContactTime;
|
|
|
|
// New init time
|
|
second._InitTime = desc.ContactTime;
|
|
}
|
|
else
|
|
{
|
|
// No retriever used
|
|
second._Position.setPos (collisionPosition);
|
|
|
|
// Position at t=0
|
|
second._3dInitPosition = collisionPosition - newSpeed * desc.ContactTime;
|
|
|
|
// New init time
|
|
second._InitTime = desc.ContactTime;
|
|
}
|
|
|
|
// Dirt pos
|
|
second.dirtPos (container, &otherPrimitive, secondWorldImage);
|
|
}
|
|
}
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
void CPrimitiveWorldImage::reaction (const CCollisionSurfaceDesc& surfaceDesc, const UGlobalPosition& globalPosition,
|
|
CGlobalRetriever& retriever, double /* ratio */, double dt, CMovePrimitive &primitive, CMoveContainer &container,
|
|
uint8 worldImage)
|
|
{
|
|
// H_AUTO(PACS_PWI_reaction_short);
|
|
|
|
// Reaction type
|
|
uint32 type=primitive.getReactionTypeInternal();
|
|
|
|
// Reaction to the collision: copy the CGlobalRetriever::CGlobalPosition
|
|
_Position.setGlobalPos (globalPosition, retriever);
|
|
|
|
// Relfexion or slide ?
|
|
if ((type==UMovePrimitive::Reflexion)||(type==UMovePrimitive::Slide))
|
|
{
|
|
// Slide ?
|
|
if (type==UMovePrimitive::Slide)
|
|
{
|
|
// Project last delta on plane of collision.
|
|
_Speed-= surfaceDesc.ContactNormal*(surfaceDesc.ContactNormal*_Speed-NELPACS_DIST_BACK/(dt-surfaceDesc.ContactTime));
|
|
}
|
|
|
|
// Reflexion ?
|
|
if (type==UMovePrimitive::Reflexion)
|
|
{
|
|
// Project last delta on plane of collision.
|
|
double speedProj=surfaceDesc.ContactNormal*_Speed;
|
|
_Speed-=surfaceDesc.ContactNormal*(speedProj+speedProj*primitive.getAttenuation()-NELPACS_DIST_BACK/(dt-surfaceDesc.ContactTime));
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Stop ?
|
|
if (type==UMovePrimitive::Stop)
|
|
{
|
|
_Speed.set (0,0,0);
|
|
}
|
|
}
|
|
|
|
// Contact time
|
|
double contactTime=surfaceDesc.ContactTime;
|
|
|
|
// Init position
|
|
_3dInitPosition = _Position.getPos() - _Speed * contactTime;
|
|
|
|
// Set contactTime
|
|
_InitTime=contactTime;
|
|
|
|
// Dirt pos
|
|
dirtPos (&container, &primitive, worldImage);
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
void CPrimitiveWorldImage::setGlobalPosition (const UGlobalPosition& pos, CMoveContainer& container, CMovePrimitive &primitive, uint8 worldImage)
|
|
{
|
|
// Cast type
|
|
nlassert (dynamic_cast<const CMoveContainer*>(&container));
|
|
const CMoveContainer *cont=(const CMoveContainer*)&container;
|
|
|
|
if (!cont->getGlobalRetriever()) return;
|
|
// Use the global retriever ?
|
|
nlassert (cont->getGlobalRetriever());
|
|
|
|
// Get the pos
|
|
_Position.setGlobalPos (pos, *cont->getGlobalRetriever());
|
|
|
|
// Precalc some values
|
|
_3dInitPosition = _Position.getPos ();
|
|
_InitTime = 0;
|
|
|
|
// Speed NULL
|
|
_Speed=CVector::Null;
|
|
|
|
// Dirt BB
|
|
dirtPos (&container, &primitive, worldImage);
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
void CPrimitiveWorldImage::setGlobalPosition (const NLMISC::CVectorD& pos, CMoveContainer& container, CMovePrimitive &primitive, uint8 worldImage, bool keepZ /*= false*/, UGlobalPosition::TType type /* =UGlobalPosition::Unspecified*/)
|
|
{
|
|
// Cast type
|
|
nlassert (dynamic_cast<const CMoveContainer*>(&container));
|
|
const CMoveContainer *cont=(const CMoveContainer*)&container;
|
|
|
|
// Get the retriever
|
|
CGlobalRetriever *retriever=cont->getGlobalRetriever();
|
|
|
|
// Use a global retriever
|
|
if (retriever)
|
|
{
|
|
// Get a cvector
|
|
// CVector vect=pos; // better with CVectorD
|
|
|
|
// Get global position
|
|
UGlobalPosition globalPosition=retriever->retrievePosition (pos, 1.0e10, type);
|
|
|
|
if (keepZ)
|
|
{
|
|
// Set the position
|
|
_Position.setPos (pos);
|
|
|
|
// Set global position
|
|
_Position.setGlobalPosKeepZ (globalPosition, *retriever);
|
|
}
|
|
else
|
|
{
|
|
// Set global position
|
|
_Position.setGlobalPos (globalPosition, *retriever);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Set the position
|
|
_Position.setPos (pos);
|
|
}
|
|
|
|
// Precalc some values
|
|
_3dInitPosition = _Position.getPos ();
|
|
_InitTime = 0;
|
|
|
|
// Speed NULL
|
|
_Speed=CVector::Null;
|
|
|
|
// Dirt BB
|
|
dirtPos (&container, &primitive, worldImage);
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
void CPrimitiveWorldImage::move (const NLMISC::CVectorD& speed, CMoveContainer& container, CMovePrimitive &primitive, uint8 worldImage)
|
|
{
|
|
// New speed
|
|
setSpeed (speed, &container, &primitive, worldImage);
|
|
|
|
// Set initial position
|
|
_3dInitPosition = _Position.getPos ();
|
|
|
|
// Set initial time
|
|
_InitTime = 0;
|
|
|
|
// Dirt BB
|
|
dirtPos (&container, &primitive, worldImage);
|
|
}
|
|
|
|
// ***************************************************************************
|
|
|
|
|
|
} // NLPACS
|