void CBunny::Update( float deltaTime )
{
// Do specific stuff, and optionally call the base class, perhaps
// Like following things...
switch ( this->m_CurrentState )
{
case CBunny::IS_FOLLOWING_CLOSEST_OBJECT: // Only ints or enum (which is an int... Shhh, don't tell anyone)
{ // **** STARTOF: ADDED to get around scope operator declaration rules *****
// Find the closest object
std::vector< unsigned int > vecIDs;
vecIDs = this->m_pMediator->GetIDOfNearestObjects( this->position, 20.0f );
// Did we find anything close?
if ( !vecIDs.empty() )
{ // Yup, we did, so pick the first
// (m_closestObject is a CVector3f)
this->m_pMediator->GetPositionByID( vecIDs[0], this->m_closestObject );
// Do the "following" stuff.
float distance = getDistanceBetweenPoints( this->position, this->m_closestObject );
// Are we close enough
if ( distance >= this->m_followClosestDistance )
{ // Move closer
this->m_followAtSpeed( this->m_closestObject, this->m_followSpeed * deltaTime );
} // if ( distance >= this->m_followClosestDistance )
} // if ( !vecIDs.empty() )
} // **** ENDOF: ADDED to get around scope operator declaration rules *****
break;
case CBunny::IS_FOLLOWING_SPECIFIC_OBJECT:
{
// Get the position of the object we are supposed to follow...
this->m_pMediator->GetPositionByID( this->m_ID_of_ObjectToFollow, this->m_closestObject );
// Do the "following" stuff.
float distance = getDistanceBetweenPoints( this->position, this->m_closestObject );
// Are we close enough
if ( distance >= this->m_followClosestDistance )
{ // Move closer
this->m_followAtSpeed( this->m_closestObject, this->m_followSpeed * deltaTime );
} // if ( distance >= this->m_followClosestDistance )
}
break;
};
CGameObject::Update( deltaTime );
return;
}
double Odometer::getDistanceToGoal() {
// Serial.print("x distance: ");
// Serial.println(GoalX - X);
// Serial.print("y distance: ");
// Serial.println(GoalY - Y);
return getDistanceBetweenPoints(X, Y, goalX, goalY);
}
double Odometer::getDistanceFromMarkedPoint() {
return getDistanceBetweenPoints(X, Y, markX, markY);
}
/* Function: getMeshFlows
*
* Description: Finds the flow of each point in the mesh by finding the k closest
* mesh points in the sebsequent frame and finding a weighted average of those
* flows, based on their distances to the mesh point.
*
* Parameters:
* flows: output Vector array to contain the deltas of the flows
* f1MeshPoints: input CvPoint3D32f array of mesh points from frame 1
* f1NumMeshPoints: number of mesh points from frame 1
* f2MeshPoints: input CvPoint3D32f array of mesh points from frame 2
* f2NumMeshPoints: number of mesh points from frame 2
*/
void getMeshFlows(
_Out_ Vector *flows,
_In_ CvPoint3D32f *f1MeshPoints,
_In_ int f1NumMeshPoints,
_In_ CvPoint3D32f *f2MeshPoints,
_In_ int f2NumMeshPoints)
{
// go through each frame 2 mesh point
for (int i = 0; i < f2NumMeshPoints; i++)
{
CvPoint3D32f f2CurMeshPoint = f2MeshPoints[i];
double shortestDistances[K];
int shortestDistanceIndices[K];
// find the mesh point from frame 1 that is closest to the current frame
// 2 mesh point k times, excluding ones already found to be closer
for (int j = 0; j < K; j++)
{
int shortestDistanceIndex;
double shortestDistance = DBL_MAX;
// go through the mesh points from frame 1
for (int n = 0; n < f1NumMeshPoints; n++)
{
bool indexUsed = false;
// make sure that the current frame 1 mesh point isn't already
// contained in the list of closest feature points to the frame
// 2 mesh point
for (int m = 0; m < j; m++)
{
if (shortestDistanceIndices[m] == n)
{
indexUsed = true;
break;
}
}
// if current frame 1 mesh point is already contained in list of
// closest feature points to the frame 2 mesh point, skip to the
// next frame 1 mesh point
if (indexUsed)
{
continue;
}
// get the distance from frame 2 mesh point to current frame 1
// mesh point
double curDistance = getDistanceBetweenPoints(f2CurMeshPoint, f1MeshPoints[n]);
// if distance is the shortest distance so far, set it as the
// shortest distance, and save the index of the frame 1 mesh point
if (curDistance < shortestDistance)
{
shortestDistance = curDistance;
shortestDistanceIndex = n;
}
}
bool insertAtEnd = true;
// insert the closest frame 1 mesh point found in this iteration into
// the sorted array of k closest points
for (int n = 0; n < j; n++)
{
if (shortestDistance < shortestDistances[n])
{
for (int m = j; m > n; n--)
{
shortestDistances[m] = shortestDistances[m-1];
shortestDistanceIndices[m] = shortestDistanceIndices[m-1];
}
shortestDistances[n] = shortestDistance;
shortestDistanceIndices[n] = shortestDistanceIndex;
insertAtEnd = false;
break;
}
}
if (insertAtEnd)
{
shortestDistances[j] = shortestDistance;
shortestDistanceIndices[j] = shortestDistanceIndex;
}
}
// create an array of weights, where the indices correspond to the indices
// of the sorted array of closest points
double weights[K];
// we need to get the sum of the weights to find the percent to multiply
// each flow between the mesh point of frames 1 and 2 to to find the
// final flow
double weightSum = 0.0;
for (int j = 0; j < K; j++)
{
// set the weight to be the distance of the k-th furthest frame 1
// mesh point from the frame 2 mesh point divided by the distance
// of the frame 2 mesh point from the frame 1 mesh point
weights[j] = shortestDistances[K-1]/shortestDistances[j];
weightSum += weights[j];
}
double deltaX = 0.0;
double deltaY = 0.0;
double deltaZ = 0.0;
for (int j = 0; j < K; j++)
{
double weightFraction = weights[j]/weightSum;
// deltas for mesh flow point is determined by the sum of the weight
// fractions mulitiplied by the flow between the k closest mesh points
// between frames 1 and 2
deltaX += weightFraction * (f1MeshPoints[shortestDistanceIndices[j]].x - f2CurMeshPoint.x);
deltaY += weightFraction * (f1MeshPoints[shortestDistanceIndices[j]].y - f2CurMeshPoint.y);
deltaZ += weightFraction * (f1MeshPoints[shortestDistanceIndices[j]].z - f2CurMeshPoint.z);
}
flows[i].deltaX = deltaX;
flows[i].deltaY = deltaY;
flows[i].deltaZ = deltaZ;
}
}
