Void CharEntity::_PathTracking_End()
{
Assert( m_pPathController != NULL );
// Disable path controller
KinematicBody * pBody = (KinematicBody*)m_pBody;
m_pPathController->Enabled = false;
pBody->DetachMotionController();
PhysicsFn->DestroyController( m_pPathController );
m_pPathController = NULL;
// Enable character controller
m_pController->Enabled = true;
pBody->AttachController( m_pController );
// Destroy path data
EntityFn->SelectMemory( TEXT("Scratch") );
Delete( m_pAngularTravel );
m_pAngularTravel = NULL;
Delete( m_pLinearTravel );
m_pLinearTravel = NULL;
Delete( m_pAngularPath );
m_pAngularPath = NULL;
Delete( m_pLinearPath );
m_pLinearPath = NULL;
EntityFn->UnSelectMemory();
}
Void CharEntity::_PathTracking_Update( const Array<WorldPathWaypoint*> & arrPath )
{
// Destroy path tracking
_PathTracking_End();
// Find which waypoint we need to go to next
Vertex3 vPosition = GetPosition();
Scalar fSqrDistance, fPrevSqrDistance = SCALAR_INFINITE;
//Scalar fPrevPrevSqrDistance;
Assert( m_iLastPassedWaypoint != INVALID_OFFSET );
UInt iNextWaypoint = m_iLastPassedWaypoint;
while( iNextWaypoint < arrPath.Count() ) {
// Detect if the path stops getting closer from our position
fSqrDistance = ( arrPath[iNextWaypoint]->GetPosition() - vPosition ).NormSqr();
if ( fSqrDistance > fPrevSqrDistance ) {
// Either one of those 2 cases : we allways choose i
// -----*--X--|-----*----------*----
// i-1 i
// -----*-----|--X--*----------*---- where i is iNextWaypoint
// i-2 i-1 i
//if ( iNextWaypoint >= 2 ) {
// fPrevPrevSqrDistance = ( arrPath[iNextWaypoint-2]->GetPosition() - vPosition ).NormSqr();
// if ( fPrevPrevSqrDistance < fSqrDistance )
// --iNextWaypoint;
//}
break;
}
fPrevSqrDistance = fSqrDistance;
++iNextWaypoint;
}
// This is a safe assumption ...
// We allways have the last call to this made way before
// path tracking is finished, timing is obviously enforced here.
Assert( iNextWaypoint < arrPath.Count() );
m_iLastPassedWaypoint = ( iNextWaypoint - 1 );
WorldPathFinder * pPathFinder = WorldFn->GetPathFinder();
Bool bFinished = pPathFinder->IsFinished( m_idPathTracking );
// Recreate path tracking
EntityFn->SelectMemory( TEXT("Scratch") );
UInt iControlPointCount = ( arrPath.Count() - iNextWaypoint + 1 );
UInt iDegree = Min<UInt>( iControlPointCount - 1, 3 );
Vertex3 * arrControlPoints = New Vertex3[iControlPointCount];
Scalar * arrParameters = New Scalar[iControlPointCount];
Vertex2 * arrTravelPoints = New Vertex2[iControlPointCount];
arrControlPoints[0] = vPosition;
for( UInt i = 1; i < iControlPointCount - 1; ++i )
arrControlPoints[i] = arrPath[iNextWaypoint + (i-1)]->GetPosition();
if ( bFinished )
arrControlPoints[iControlPointCount - 1] = m_vPathTarget;
else
arrControlPoints[iControlPointCount - 1] = arrPath[iNextWaypoint + (iControlPointCount - 2)]->GetPosition();
m_pLinearPath = New BSplinePatch3( arrControlPoints, iControlPointCount, iDegree, false, false );
m_pLinearPath->SubDivideByParameter( arrParameters, iControlPointCount );
for( UInt i = 0; i < iControlPointCount; ++i ) {
Vector3 vTangent = m_pLinearPath->Tangent( arrParameters[i] );
arrControlPoints[i].X = 0.0f;
arrControlPoints[i].Y = 0.0f;
arrControlPoints[i].Z = MathFn->ArcCos( vTangent * Vector3::eI );
}
m_pAngularPath = New BSplinePatch3( arrControlPoints, iControlPointCount, iDegree, false, false );
Scalar fLength = m_pLinearPath->Curve3::Length();
Scalar fTime = ( fLength / m_pCharacter->GetMovementSpeed() );
Scalar fLengthInterval = ( fLength / (Scalar)(iControlPointCount - 1) );
Scalar fTimeInterval = ( fTime / (Scalar)(iControlPointCount - 1) );
Scalar fX = 0.0f, fY = 0.0f;
for( UInt i = 0; i < iControlPointCount; ++i ) {
arrTravelPoints[i].X = fX;
arrTravelPoints[i].Y = fY;
fX += fLengthInterval;
fY += fTimeInterval;
}
m_pLinearTravel = New BSplinePatch2( arrTravelPoints, iControlPointCount, iDegree, false, false );
m_pAngularTravel = New BSplinePatch2( arrTravelPoints, iControlPointCount, iDegree, false, false );
DeleteA( arrTravelPoints );
arrTravelPoints = NULL;
DeleteA( arrParameters );
arrParameters = NULL;
DeleteA( arrControlPoints );
arrControlPoints = NULL;
EntityFn->UnSelectMemory();
// Disable character controller
m_pController->SetStanding();
KinematicBody * pBody = (KinematicBody*)m_pBody;
m_pController->Enabled = false;
pBody->DetachMotionController();
// Enable path controller
m_pPathController = PhysicsFn->CreatePathController( TEXT("PathController"), m_pLinearPath, m_pLinearTravel,
m_pAngularPath, m_pAngularTravel, EULER_ANGLES_ZYX );
m_pPathController->Enabled = true;
m_pPathController->MaxTime = fTime;
pBody->AttachController( m_pPathController );
}
Void CharEntity::_PathTracking_Start( const Array<WorldPathWaypoint*> & arrPath )
{
Assert( m_pPathController == NULL );
WorldPathFinder * pPathFinder = WorldFn->GetPathFinder();
Bool bFinished = pPathFinder->IsFinished( m_idPathTracking );
// Create path data
EntityFn->SelectMemory( TEXT("Scratch") );
Bool bBias = false;
UInt iControlPointCount = arrPath.Count();
if ( iControlPointCount == 1 ) {
Assert( bFinished );
++iControlPointCount;
bBias = true;
}
UInt iDegree = Min<UInt>( iControlPointCount - 1, 3 );
Vertex3 * arrControlPoints = New Vertex3[iControlPointCount];
Scalar * arrParameters = New Scalar[iControlPointCount];
Vertex2 * arrTravelPoints = New Vertex2[iControlPointCount];
arrControlPoints[0] = GetPosition();
if ( bBias )
arrControlPoints[1] = m_vPathTarget;
else {
for( UInt i = 1; i < iControlPointCount - 1; ++i )
arrControlPoints[i] = arrPath[i]->GetPosition();
if ( bFinished )
arrControlPoints[iControlPointCount - 1] = m_vPathTarget;
else
arrControlPoints[iControlPointCount - 1] = arrPath[iControlPointCount - 1]->GetPosition();
}
m_pLinearPath = New BSplinePatch3( arrControlPoints, iControlPointCount, iDegree, false, false );
m_pLinearPath->SubDivideByParameter( arrParameters, iControlPointCount );
for( UInt i = 0; i < iControlPointCount; ++i ) {
Vector3 vTangent = m_pLinearPath->Tangent( arrParameters[i] );
arrControlPoints[i].X = 0.0f;
arrControlPoints[i].Y = 0.0f;
arrControlPoints[i].Z = MathFn->ArcCos( vTangent * Vector3::eI );
}
m_pAngularPath = New BSplinePatch3( arrControlPoints, iControlPointCount, iDegree, false, false );
Scalar fLength = m_pLinearPath->Curve3::Length();
Scalar fTime = ( fLength / m_pCharacter->GetMovementSpeed() );
Scalar fLengthInterval = ( fLength / (Scalar)(iControlPointCount - 1) );
Scalar fTimeInterval = ( fTime / (Scalar)(iControlPointCount - 1) );
Scalar fX = 0.0f, fY = 0.0f;
for( UInt i = 0; i < iControlPointCount; ++i ) {
arrTravelPoints[i].X = fX;
arrTravelPoints[i].Y = fY;
fX += fLengthInterval;
fY += fTimeInterval;
}
m_pLinearTravel = New BSplinePatch2( arrTravelPoints, iControlPointCount, iDegree, false, false );
m_pAngularTravel = New BSplinePatch2( arrTravelPoints, iControlPointCount, iDegree, false, false );
DeleteA( arrTravelPoints );
arrTravelPoints = NULL;
DeleteA( arrParameters );
arrParameters = NULL;
DeleteA( arrControlPoints );
arrControlPoints = NULL;
EntityFn->UnSelectMemory();
// Disable character controller
m_pController->SetStanding();
KinematicBody * pBody = (KinematicBody*)m_pBody;
m_pController->Enabled = false;
pBody->DetachMotionController();
// Enable path controller
m_pPathController = PhysicsFn->CreatePathController( TEXT("PathController"), m_pLinearPath, m_pLinearTravel,
m_pAngularPath, m_pAngularTravel, EULER_ANGLES_ZYX );
m_pPathController->Enabled = true;
m_pPathController->MaxTime = fTime;
pBody->AttachController( m_pPathController );
}
