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); }
void VART::Human::StickLeftFoot() { // Right foot is backwards. Adjustment brings it forward, so... position.CopyMatrix(adjustment * position); // ...save current adjustment to human position pathToStickPosPtr = &pathToLFoot; stickPositionPtr = &lfFront; // now the left foot sticks to ground, but it is forward ... // ... adjustment will bring it backwards, so we compute how ... Point4D leftFootOffset; // ... much forward it is ComputeLocalStickPosition(&leftFootOffset); // ... and add to position as well ... Transform trans; // ... as vector, under influence of the human's orientation. Point4D forward = position * orientation * (leftFootOffset - lfFront); forward.SetY(-adjustment.GetData()[13]); trans.MakeTranslation(forward); position.CopyMatrix(trans * position); }