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 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(point_t *p1, 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
  78 vect_norm(vect_t *v)
  79 {
  80         return sqrt(v->x*v->x+v->y*v->y);
  81 }
  82
  83 void vect_rot_trigo(vect_t *v)
  84 {
  85         float s;
  86
  87         s = v->x;
  88         v->x= -v->y;
  89         v->y = s;
  90 }
  91
  92 void vect_rot_retro(vect_t *v)
  93 {
  94         float s;
  95
  96         s = v->x;
  97         v->x= v->y;
  98         v->y = -s;
  99 }
 100
 101
 102 float vect_get_angle(vect_t *v, vect_t *w)
 103 {
 104         float ps;
 105         float n;
 106
 107         ps = vect_pscal(v, w);
 108         n = vect_norm(v) * vect_norm(w);
 109
 110         return acos((float)ps/n);
 111 }
 112
 113 void vect_resize(vect_t *v, float l)
 114 {
 115         float old_l = vect_norm(v);
 116         float x = v->x, y = v->y;
 117         v->x = x * l / old_l;
 118         v->y = y * l / old_l;
 119 }