modules/base/math/geometry/vect_base.c

   1 /*
   2  *  Copyright Droids Corporation (2009)
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License as published by
   6  *  the Free Software Foundation; either version 2 of the License, or
   7  *  (at your option) any later version.
   8  *
   9  *  This program is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  *  GNU General Public License for more details.
  13  *
  14  *  You should have received a copy of the GNU General Public License
  15  *  along with this program; if not, write to the Free Software
  16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  17  *
  18  *  Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
  19  *
  20  */
  21
  22 #include <stdint.h>
  23 #include <math.h>
  24 #include <vect_base.h>
  25
  26 /* Return scalar product */
  27 float
  28 vect_pscal(vect_t *v, vect_t *w)
  29 {
  30         return v->x * w->x + v->y * w->y;
  31 }
  32
  33 /* Return Z of vectorial product */
  34 float
  35 vect_pvect(vect_t *v, vect_t *w)
  36 {
  37         return v->x*w->y - v->y*w->x;
  38 }
  39
  40 /* Return scalar product */
  41 int8_t
  42 vect_pscal_sign(vect_t *v, vect_t *w)
  43 {
  44         float z;
  45         z = vect_pscal(v, w);
  46         if (z==0)
  47                 return 0;
  48         return z>0?1:-1;
  49 }
  50
  51 /* Return Z of vectorial product */
  52 int8_t
  53 vect_pvect_sign(vect_t *v, vect_t *w)
  54 {
  55         float z;
  56         z = vect_pvect(v, w);
  57         if (z==0)
  58                 return 0;
  59         return z>0?1:-1;
  60 }
  61
  62 float xy_norm(float x1, float y1, float x2, float y2)
  63 {
  64         float x = x2 - x1;
  65         float y = y2 - y1;
  66         return sqrt(x*x + y*y);
  67 }
  68
  69 float pt_norm(const point_t *p1, const point_t *p2)
  70 {
  71         float x = p2->x - p1->x;
  72         float y = p2->y - p1->y;
  73         return sqrt(x*x + y*y);
  74 }
  75
  76 /* norm of a vector */
  77 float vect_norm(const vect_t *v)
  78 {
  79         return sqrt(v->x*v->x+v->y*v->y);
  80 }
  81
  82 void vect_rot_trigo(vect_t *v)
  83 {
  84         float s;
  85
  86         s = v->x;
  87         v->x= -v->y;
  88         v->y = s;
  89 }
  90
  91 void vect_rot_retro(vect_t *v)
  92 {
  93         float s;
  94
  95         s = v->x;
  96         v->x= v->y;
  97         v->y = -s;
  98 }
  99
 100
 101 float vect_get_angle(vect_t *v, vect_t *w)
 102 {
 103         float ps;
 104         float n;
 105
 106         ps = vect_pscal(v, w);
 107         n = vect_norm(v) * vect_norm(w);
 108
 109         return acos((float)ps/n);
 110 }
 111
 112 void vect_resize(vect_t *v, float l)
 113 {
 114         float old_l = vect_norm(v);
 115         float x = v->x, y = v->y;
 116         v->x = x * l / old_l;
 117         v->y = y * l / old_l;
 118 }