geometry.cpp

RoboCup 3D 仿真组清华大学2005的源代码

/***************************************************************************
 *   Copyright (C) 2005 by Rujia Liu                                       *
 *   [e-mail]                                                  *
 *                                                                         *
 *   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; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   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.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "geometry.h"

namespace Geometry{

    // auxiliary functions
    float rand_float(float min, float max)
    {
        int r = rand();
        float range = max - min;
        return (float)r / (float)RAND_MAX * range + min;
    }

    float sqr(float x)
    {
        return x*x;
    }
    
    // basic geometry operations: cross product, dot product and distance
    float cross(Point p0, Point p1, Point p2)
    {
        return (p1[0]-p0[0])*(p2[1]-p0[1])-(p2[0]-p0[0])*(p1[1]-p0[1]);
    }
    
    float dot(Point p0, Point p1, Point p2)
    {
        return (p1[0]-p0[0])*(p2[0]-p0[0])+(p1[1]-p0[1])*(p2[1]-p0[1]);
    }
    
    float dis(Point p1, Point p2)
    {
        return sqrt(sqr(p1[0]-p2[0])+sqr(p1[1]-p2[1]));
    }
       
    // geometry objects
    Point MakePoint(Vector3f v)
    {
        Vector2f v2;
        v2[0] = v[0];
        v2[1] = v[1];
        return v2;
    }
    
    Ray MakeRay(Point origin, Point direction)
    {
        Ray r;
        r.o = origin;
        r.dir = direction;
        r.dir.Normalize();
        return r;
    }
    
    float GetDistOnRay(Ray r, Point p)
    {
        if(fabs(r.dir[0]) < eps && fabs(r.dir[1]) < eps) return 0.0f;

        // these two values should be the same theoretically, this ways reduces error only.
        if(fabs(r.dir[0]) > fabs(r.dir[1]))
            return (p[0] - r.o[0]) / r.dir[0];
        else
            return (p[1] - r.o[1]) / r.dir[1];
    }
    
    /* 
       Given a ray p0->p1 what is the point whose distance(signed) to p0 is dist?
    
       p0--------->p1                            ?????
        \---------------------dist ---------------/
    */
    Point GetPointOnRay(Point p0, Point p1, float dist)
    {
        float d = dis(p0, p1);
        if(fabs(d) < 1e-3) return p0; // dunno how to calculate: p0 and p1 are the same. should give a warning
        Point pp;
        if(fabs(p0[0] - p1[0]) < 1e-3) pp[0] = p0[0]; else pp[0] = p0[0] + (p1[0] - p0[0])/d*dist;
        if(fabs(p0[1] - p1[1]) < 1e-3) pp[1] = p0[1]; else pp[1] = p0[1] + (p1[1] - p0[1])/d*dist;
        return pp;
    }
    
    // pol --> cart convertor
    Vector3f GetRelativePosition(const VisionSense& vision)
    {
        return Vector3f(
            vision.distance * gCos(gDegToRad(vision.theta)) * gCos(gDegToRad(vision.phi)),
            vision.distance * gSin(gDegToRad(vision.theta)) * gCos(gDegToRad(vision.phi)),
            vision.distance * gSin(gDegToRad(vision.phi)) );
    }
    
    /*
       line
            x  (positive distance)
          /
       /
      p1-------p2
       \
         \
           \
             x (negative distance)
    */
    float DistanceToLine(Point x, Point p1, Point p2, Point& p)
    {
        float d = dis(p1, p2);
        float s = cross(p1, p2, x) / d;
        float t = dot(p1, p2, x) / dot(p1, p2, p2);
        p[0] = p1[0] + t * (p2[0] - p1[0]);
        p[1] = p1[1] + t * (p2[1] - p1[1]);
        return s; // signed
    }

    /*
        vector p0-p's projection length on p0-p1. (signed value of p0->p')
                   p
                 /
               /  
             /            
           p0------p'----------------->p1
    
       - GetProjectionDistance(p0, p1, p) returns the SIGNED distance of p0 to p'
       - GetProjectionPoint(p0, p1, p) returns the point p'
    */
    float GetProjectionDistance(Point p0, Point p1, Point p)
    {
        return dot(p0, p1, p) / dis(p0, p1);
    }
    Point GetProjectionPoint(Point p0, Point p1, Point p)
    {
        float d = dis(p0, p1);
        if(fabs(d) < 1e-3) return p0; // dunno how to calculate: p0 and p1 are the same.	
        float dist = GetProjectionDistance(p0, p1, p);
        return GetPointOnRay(p0, p1, dist);
    }
    
    /*
        direction op's value is r(signed), tangent direction is t(signed)

        Q
        ^
        |
     t  |
        |
        O--------------------> P
                    r
  
        VectorCombine(p, t, r) returns sum of vectors P + Q
    */
    Point VectorCombine(Point p, float t, float rr)
    {
        Point unit_r = p.Normalized();
        Point unit_t;
        unit_t[0] = -unit_r[1];
        unit_t[1] = unit_r[0];
        return unit_r * rr + unit_t * t;
    }    
}