Changeset 1954 in orxonox.OLD for orxonox

Timestamp:

Jun 15, 2004, 4:52:33 PM (20 years ago)

Author:

chris

Message:

orxonox/branches/chris: Zusätzliche Vektorgeometriefunktionen implementiert (Ebenen, Linien, etc.)

Location:

orxonox/branches/chris/core

Files:

• vector.cc (modified) (1 diff)
• vector.h (modified) (2 diffs)

orxonox/branches/chris/core/vector.cc

@@ -168 +168 @@
   return f * 180 / PI;
 }
+
+/**
+   \brief create a rotation from a vector
+   \param v: a vector
+*/
+Rotation::Rotation (const Vector& v)
+{
+  Vector x = Vector( 1, 0, 0);
+  Vector axis = x.cross( v);
+  axis.normalize();
+  float angle = angle_rad( x, v);
+  float ca = cos(angle);
+  float sa = sin(angle);
+  m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
+  m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
+  m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
+  m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
+  m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
+  m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
+  m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
+  m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
+  m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
+}
+
+/**
+   \brief creates a rotation from an axis and an angle (radians!)
+   \param axis: the rotational axis
+   \param angle: the angle in radians
+*/
+Rotation::Rotation (const Vector& axis, float angle)
+{
+  float ca, sa;
+  ca = cos(angle);
+  sa = sin(angle);
+  m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
+  m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
+  m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
+  m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
+  m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
+  m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
+  m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
+  m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
+  m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
+}
+
+/**
+   \brief creates a rotation from euler angles (pitch/yaw/roll)
+   \param pitch: rotation around z (in radians)
+   \param yaw: rotation around y (in radians)
+   \param roll: rotation around x (in radians)
+*/
+Rotation::Rotation ( float pitch, float yaw, float roll)
+{
+  float cy, sy, cr, sr, cp, sp;
+  cy = cos(yaw);
+  sy = sin(yaw);
+  cr = cos(roll);
+  sr = sin(roll);
+  cp = cos(pitch);
+  sp = sin(pitch);
+  m[0] = cy*cr;
+  m[1] = -cy*sr;
+  m[2] = sy;
+  m[3] = cp*sr+sp*sy*cr;
+  m[4] = cp*cr-sp*sr*sy;
+  m[5] = -sp*cy;
+  m[6] = sp*sr-cp*sy*cr;
+  m[7] = sp*cr+cp*sy*sr;
+  m[8] = cp*cy;
+}
+
+/**
+   \brief creates a nullrotation
+*/
+Rotation::Rotation ()
+{
+  m[0] = 1.0f;
+  m[1] = 0.0f;
+  m[2] = 0.0f;
+  m[3] = 0.0f;
+  m[4] = 1.0f;
+  m[5] = 0.0f;
+  m[6] = 0.0f;
+  m[7] = 0.0f;
+  m[8] = 1.0f;
+}
+
+/**
+   \brief rotates the vector by the given rotation
+   \param v: a vector
+   \param r: a rotation
+*/
+Vector rotate_vector( const Vector& v, const Rotation& r)
+{
+  Vector t;
+  
+  t.x = v.x * r.m[0] + v.y * r.m[1] + v.z * r.m[2];
+  t.y = v.x * r.m[3] + v.y * r.m[4] + v.z * r.m[5];
+  t.z = v.x * r.m[6] + v.y * r.m[7] + v.z * r.m[8];
+
+  return t;
+}
+
+/**
+   \brief calculate the distance between two lines
+   \param l: the other line
+*/
+float Line::distance (const Line& l) const
+{
+  float q, d;
+  Vector n = a.cross(l.a);
+  q = n.dot(r-l.r);
+  d = n.len();
+  if( d == 0.0) return 0.0;
+  return q/d;
+}
+
+/**
+   \brief calculate the distance between a line and a point
+   \param v: the point
+*/
+float Line::distance_point (const Vector& v) const
+{
+  Vector d = v-r;
+  Vector u = a * d.dot( a);
+  return (d - u).len();
+}
+
+/**
+   \brief calculate the two points of minimal distance of two lines
+   \param l: the other line
+*/
+Vector* Line::footpoints (const Line& l) const
+{
+  Vector* fp = new Vector[2];
+  Plane p = Plane (r + a.cross(l.a), r, r + a);
+  fp[1] = p.intersect_line (l);
+  p = Plane (fp[1], l.a);
+  fp[0] = p.intersect_line (*this);
+  return fp;
+}
+
+/**
+   \brief calculate the length of a line
+*/
+float Line::len() const
+{
+  return a.len();
+}
+
+/**
+   \brief rotate the line by given rotation
+   \param rot: a rotation
+*/
+void Line::rotate (const Rotation& rot)
+{
+  Vector t = a + r;
+  t = rotate_vector( t, rot);
+  r = rotate_vector( r, rot),
+  a = t - r;
+}
+
+/**
+   \brief create a plane from three lines
+   \param a: first point
+   \param b: second point
+   \param c: third point
+*/
+Plane::Plane (Vector a, Vector b, Vector c)
+{
+  n = (a-b).cross(c-b);
+  k = -(n.x*b.x+n.y*b.y+n.z*b.z);
+}
+
+/**
+   \brief create a plane from anchor point and normal
+   \param n: normal vector
+   \param p: anchor point
+*/
+Plane::Plane (Vector norm, Vector p)
+{
+  n = norm;
+  k = -(n.x*p.x+n.y*p.y+n.z*p.z);
+}
+
+/**
+   \brief returns the intersection point between the plane and a line
+   \param l: a line
+*/
+Vector Plane::intersect_line (const Line& l) const
+{
+  if (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z == 0.0) return Vector(0,0,0);
+  float t = (n.x*l.r.x+n.y*l.r.y+n.z*l.r.z+k) / (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z);
+  return l.r + (l.a * t);
+}
+
+/**
+   \brief returns the distance between the plane and a point
+   \param p: a Point
+*/
+float Plane::distance_point (const Vector& p) const
+{
+  float l = n.len();
+  if( l == 0.0) return 0.0;
+  return (n.dot(p) + k) / n.len();
+}
+
+/**
+   \brief returns the side a point is located relative to a plane
+   \param p: a Point
+*/
+float Plane::locate_point (const Vector& p) const
+{
+  return (n.dot(p) + k);
+}

orxonox/branches/chris/core/vector.h

@@ -3 +3 @@
 #define VECTOR_H
 
-#include "data_tank.h"
-#include "stdincl.h"
+#include <math.h>
+#define PI 3.14159265359f
 
 class Vector {
@@ -30 +30 @@
 float angle_rad (const Vector& v1, const Vector& v2);
 
+float line_distance (Vector* l1, Vector* l2);
+
+class Rotation {
+  public:
+  
+  float m[9];
+  
+  Rotation ( const Vector& v);
+  Rotation ( const Vector& axis, float angle);
+  Rotation ( float pitch, float yaw, float roll);
+  Rotation ();
+  ~Rotation () {}
+  
+};
+
+Vector rotate_vector( const Vector& v, const Rotation& r);
+
+class Line
+{
+  public:
+  
+  Vector r, a;
+  
+  Line ( Vector r, Vector a) : r(r), a(a) {}
+  Line () : r(Vector(0,0,0)), a(Vector (1,1,1)) {}
+  ~Line () {}
+  
+  float distance (const Line& l) const;
+  float distance_point (const Vector& v) const;
+  Vector* footpoints (const Line& l) const;
+  float len () const;
+  
+  void rotate(const Rotation& rot);
+};
+
+class Plane
+{
+  public:
+  
+  Vector n;
+  float k;
+  
+  Plane (Vector a, Vector b, Vector c);
+  Plane (Vector norm, Vector p);
+  Plane (Vector n, float k) : n(n), k(k) {}
+  Plane () : n(Vector(1,1,1)), k(0) {}
+  ~Plane () {}
+  
+  Vector intersect_line (const Line& l) const;
+  float distance_point (const Vector& p) const;
+  float locate_point (const Vector& p) const;
+};
 
 #endif