Esempio n. 1
0
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;
    }
}
Esempio n. 2
0
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);
}
Esempio n. 3
0
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);
}