khanat-opennel-code/code/nel/src/pacs/primitive_world_image.cpp

1491 lines
41 KiB
C++
Raw Normal View History

2010-05-06 00:08:41 +00:00
// 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 ) )
2010-05-06 00:08:41 +00:00
*
* 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
2010-05-06 00:08:41 +00:00
*
* a = Norm^2 (B)
2010-05-06 00:08:41 +00:00
* b = 2*(A.B)
* c = Norm^2 (A) - R^2
2010-05-06 00:08:41 +00:00
*
* a*t^2 + b*t + c = 0
2010-05-06 00:08:41 +00:00
*/
// 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 ) )
2010-05-06 00:08:41 +00:00
*
* 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
2010-05-06 00:08:41 +00:00
*
* a = Norm^2 (B)
2010-05-06 00:08:41 +00:00
* b = 2*(A.B)
* c = Norm^2 (A) - (R + R')^2
2010-05-06 00:08:41 +00:00
*
* a*t^2 + b*t + c = 0
2010-05-06 00:08:41 +00:00
*/
// 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);
2010-05-06 00:08:41 +00:00
// 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