float VART::Human::AngleToPosition(const Point4D& pos, float* totalAngle) const
// Compute Angle to a position (pos) in World coordinates
{
static const float delta = 3.5; // distances smaller than delta are to small to make sharp turns
Point4D direction = pos - Position();
direction.SetY(0);
float stepAngle = maxStepRotation;
if (direction.Length() < delta) // if too close to destination, make only a small adjustment
stepAngle *= 0.01;
float angle = direction.GenericAngleTo(forward);
// Compute the Y coordinate of the cross product (we are working on world coordinates)
// between forward and direction. If it is negative, the angle is also negative.
if ((forward.GetZ()*direction.GetX() - forward.GetX()*direction.GetZ()) < 0)
{ // Negative angle
if (totalAngle)
*totalAngle = -angle;
if (angle > stepAngle)
return -stepAngle;
else
return -angle;
}
else
{ // Positive angle
if (totalAngle)
*totalAngle = angle;
if (angle > stepAngle)
return stepAngle;
else
return angle;
}
}
bool VART::Human::ReachedDestiantion() const
{
static const float delta = 1.4;
Point4D location = adjustment * position * orientation * Point4D::ORIGIN();
Point4D distanceVector = destination - location;
//cout << "From " << location << " to " << destination << "\n";
distanceVector.SetY(0);
float distance = abs(distanceVector.Length());
//~ cout << "distance: " << distance << "\n";
return (distance < delta);
}
