geometry.cpp

agentspark 机器人模拟代码 适用robocup 机器人步态模拟仿真（机器人动作在NAOGETUP.cpp下修改）

/*
   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "geometry.h"

using namespace std;
using namespace salt;

Matrix RotationMatrixFrom2Vec(Vector3f from, Vector3f to)
{
    Matrix m;
    m.Identity();
    from.Normalize();
    to.Normalize();

    Vector3f axis = from.Cross(to);
    float angle = gArcCos(from.Dot(to));

    float c = gCos(angle);
    float s = gSin(angle);
    float t = 1.0 - c;
    m(0, 0) = c + axis.x()*axis.x()*t;
    m(1, 1) = c + axis.y()*axis.y()*t;
    m(2, 2) = c + axis.z()*axis.z()*t;

    float tmp1 = axis.x()*axis.y()*t;
    float tmp2 = axis.z()*s;
    m(1, 0) = tmp1 + tmp2;
    m(0, 1) = tmp1 - tmp2;
    tmp1 = axis.x()*axis.z()*t;
    tmp2 = axis.y()*s;
    m(2, 0) = tmp1 - tmp2;
    m(0, 2) = tmp1 + tmp2;
    tmp1 = axis.y()*axis.z()*t;
    tmp2 = axis.x()*s;
    m(2, 1) = tmp1 + tmp2;
    m(1, 2) = tmp1 - tmp2;

    return m;
}

bool InverseMatrix(salt::Matrix& m)
{
    float det = m(0, 0) * m(1, 1) * m(2, 2) +
                m(0, 1) * m(1, 2) * m(2, 0) +
                m(0, 2) * m(1, 0) * m(2, 1) -
                m(0, 0) * m(1, 2) * m(2, 1) -
                m(0, 1) * m(1, 0) * m(2, 2) -
                m(0, 2) * m(1, 1) * m(2, 0);

    if (fabs(det) < 1e-4) return false;

    det = 1.0f / det;

    Matrix mat;
    mat.Identity();
    mat(0, 0) = (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) * det;
    mat(1, 0) = (m(1, 2) * m(2, 0) - m(1, 0) * m(2, 2)) * det;
    mat(2, 0) = (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0)) * det;
    mat(0, 1) = (m(0, 2) * m(2, 1) - m(0, 1) * m(2, 2)) * det;
    mat(1, 1) = (m(0, 0) * m(2, 2) - m(0, 2) * m(2, 0)) * det;
    mat(2, 1) = (m(0, 1) * m(2, 0) - m(0, 0) * m(2, 1)) * det;
    mat(0, 2) = (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)) * det;
    mat(1, 2) = (m(0, 2) * m(1, 0) - m(0, 0) * m(1, 2)) * det;
    mat(2, 2) = (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0)) * det;
    m = mat;

    return true;
}

bool IsRi(const salt::Matrix& R)
{
    float ran = 1e-3;
    if (gAbs(R(0,0) - 1.0) > ran ||
        gAbs(R(0,1)      ) > ran ||
        gAbs(R(0,2)      ) > ran ||
        gAbs(R(1,0)      ) > ran ||
        gAbs(R(1,1) - 1.0) > ran ||
        gAbs(R(1,2)      ) > ran ||
        gAbs(R(2,0)      ) > ran ||
        gAbs(R(2,1)      ) > ran ||
        gAbs(R(2,2) - 1.0) > ran
        )
    {
        return false;
    }

    return true;
}

salt::Vector3f RotToOmega(const salt::Matrix& R)
{
    Vector3f w(0, 0, 0);
    float theta = (R(0,0) + R(1,1) + R(2,2) - 1) / 2;
    float gain;

    if (! (IsRi(R) || gAbs(theta) > 1.0))
    {
        theta = gArcCos(theta);
        gain = 0.5 * theta / gSin(theta);
        w[0] = gain * (R(2,1) - R(1,2));
        w[1] = gain * (R(0,2) - R(2,0));
        w[2] = gain * (R(1,0) - R(0,1));
    }
    return w;
}

Matrix Rodrigues(const Vector3f& axis, float theta)
{
    float a = gSin(theta);
    float b = 1.0f - gCos(theta);

    Matrix x1, x2, y;
    x1.Set(       0, -axis.z(),  axis.y(), 0,
           axis.z(),         0, -axis.x(), 0,
          -axis.y(),  axis.x(),         0, 0,
                  0,         0,         0, 0
          );
    x2 = x1 * x1;

    y.Set(1.0f + a*x1(0,0) + b*x2(0,0),
                 a*x1(0,1) + b*x2(0,1),
                 a*x1(0,2) + b*x2(0,2),
                                     0,
                 a*x1(1,0) + b*x2(1,0),
          1.0f + a*x1(1,1) + b*x2(1,1),
                 a*x1(1,2) + b*x2(1,2),
                                     0,
                 a*x1(2,0) + b*x2(2,0),
                 a*x1(2,1) + b*x2(2,1),
          1.0f + a*x1(2,2) + b*x2(2,2),
                                     0,
                                     0,
                                     0,
                                     0,
                                     0
          );
    return y;
}

/*!
    \fn   Cross
    cross operator on vectors
    Basic funtion ,used by other functions in Geomotry
    You may find the formula:  x1 * y2 - x2 * y1 in linear algebra book 

    \param   O      the start point of both vectors

    \param   end1   the end point of one vector

    \param   end2   the end point of the other vector

    @Author: <Rujia Liu [e-mail]>
*/
float Cross(const Vector2f& O, const Vector2f& end1, const Vector2f& end2)
{
    float x1 = end1.x() - O.x();
    float x2 = end2.x() - O.x();
    float y1 = end1.y() - O.y();
    float y2 = end2.y() - O.y();

    return x1 * y2 - x2 * y1;
}

/*!
    \fn Dot

    dot operator on vectors
    Basic funtion ,used by other functions in Geomotry
    you may find the formula:  x1 * y2 + x2 * y1 in linear algebra book 

    \param   O    \ the start point of both vectors

    \param   end1 \ the end point of one vector

    \param   end2 \ the end point of the other vector

    @Author: <Rujia Liu [e-mail]>
*/
float Dot(const Vector2f& O, const Vector2f& end1, const Vector2f& end2)
{
    float x1 = end1.x() - O.x();
    float x2 = end2.x() - O.x();
    float y1 = end1.y() - O.y();
    float y2 = end2.y() - O.y();

    return x1 * x2 + y1 * y2;
}

/*!
    \fn DistanceToLine

    To Get the shortest distance from a point to a line

    \param p         \the point

    \param lineStart \the start point of the line

    \param lineEnd   \the end point of the line

    \param &proj     \the projection point on the line

    @Author: <Rujia Liu [e-mail]>

            p  <positive distance>
          /
       /
    lineStart-------lineEnd
       \
         \
           \
             p <negative distance>
*/
float DistanceToLine(const Vector2f& p, const Vector2f& lineStart,
                     const Vector2f& lineEnd, Vector2f& proj)
{
    float d = (lineStart - lineEnd).Length();

//     assert(fabs(d) > 0);
    float disToLine = Cross(lineStart, lineEnd, p) / d;

    float t = Dot(lineStart,lineEnd, p) / Dot(lineStart,lineEnd, lineEnd);
    proj = lineStart +  (lineEnd - lineStart) * t;

    return disToLine; // signed
}

/*!
    \fn  GetProjectionDistance

    To Get the signed value of distance from lineStart point to projection point

    \param p         \the point

    \param lineStart \the start point of the line

    \param lineEnd   \the end point of the line

    @Author: <Rujia Liu [e-mail]>
                   p
                 /
               /
             /
     lineStart------proj----------------->lineEnd
     <positive value>
*/
float GetProjectionDistance(const Vector2f& p, const Vector2f& lineStart,
                            const Vector2f& lineEnd)
{
    // assert((lineStart - lineEnd).Length() > 1e-05);
    return Dot(lineStart, lineEnd, p) / (lineStart - lineEnd).Length();
}

/*!
    \fn  IsObjectInTheWay

    To avoid obstacle, use this fn to test.

    \param objectPos the object possible to be a obstacle

    \param lineStart the start point of the line

    \param lineEnd   the end point of the line

    \param radius    minium distance from me to object to avoid collide

    @author <Lancelot [e-mail]>

                     -------
                    /       \
                   |objectPos|
     lineStart------\       /------------>lineEnd
                     -------
*/
bool IsObjectInTheWay(const Vector2f& objectPos, const Vector2f& lineStart,
                       const Vector2f& lineEnd, float radius)
{
    float dis = (lineStart - lineEnd).Length();
    Vector2f projPoint(0,0);
    float dist2Line = DistanceToLine(objectPos, lineStart, lineEnd, projPoint);
    float projDist =  GetProjectionDistance(objectPos, lineStart, lineEnd);

    return (projDist > 0 && gAbs(dist2Line) < radius && projDist < dis);
}

float GetVector2fAngleDeg(const Vector2f &v)
{
    return gRadToDeg(gArcTan2(v.y(), v.x()));
}

float GetMatrixRotateXRad(const salt::Matrix &m)
{
    return gArcTan2(m(2,1), m(2,2));