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;
}
}
void Layer::drawOrbit(double orbitAngle) {
GLfloat WHITE [] = {1.0, 1.0, 1.0};
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, WHITE);
for(int i = 0; i < 360; i+= 5) {
glBegin(GL_POINTS);
Point4D positioninOrbit = getPositionInOrbit(i, radius, orbitAngle);
glVertex3d(positioninOrbit.GetX(), positioninOrbit.GetY(), positioninOrbit.GetZ());
glEnd();
}
}
