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 ); }