Changeset 6617 in orxonox.OLD for trunk/src/lib/math/line.cc
- Timestamp:
- Jan 19, 2006, 12:45:55 PM (18 years ago)
- File:
-
- 1 copied
Legend:
- Unmodified
- Added
- Removed
-
trunk/src/lib/math/line.cc
r6616 r6617 13 13 co-programmer: Patrick Boenzli : Vector::scale() 14 14 Vector::abs() 15 16 Quaternion code borrowed from an Gamasutra article by Nick Bobick and Ken Shoemake17 18 2005-06-02: Benjamin Grauer: speed up, and new Functionality to Vector (mostly inline now)19 15 */ 20 16 21 17 #define DEBUG_SPECIAL_MODULE DEBUG_MODULE_MATH 22 18 23 #include "vector.h" 19 #include "line.h" 20 #include "plane.h" 24 21 #ifdef DEBUG 25 22 #include "debug.h" … … 30 27 31 28 using namespace std; 32 33 /////////////34 /* VECTORS */35 /////////////36 /**37 * returns the this-vector normalized to length 1.038 * @todo there is some error in this function, that i could not resolve. it just does not, what it is supposed to do.39 */40 Vector Vector::getNormalized() const41 {42 float l = this->len();43 if(unlikely(l == 1.0 || l == 0.0))44 return *this;45 else46 return (*this / l);47 }48 49 /**50 * Vector is looking in the positive direction on all axes after this51 */52 Vector Vector::abs()53 {54 Vector v(fabs(x), fabs(y), fabs(z));55 return v;56 }57 58 59 60 /**61 * Outputs the values of the Vector62 */63 void Vector::debug() const64 {65 PRINT(0)("x: %f; y: %f; z: %f", x, y, z);66 PRINT(0)(" lenght: %f", len());67 PRINT(0)("\n");68 }69 70 /**71 * create a rotation from a vector72 * @param v: a vector73 */74 Rotation::Rotation (const Vector& v)75 {76 Vector x = Vector( 1, 0, 0);77 Vector axis = x.cross( v);78 axis.normalize();79 float angle = angleRad( x, v);80 float ca = cos(angle);81 float sa = sin(angle);82 m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);83 m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;84 m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;85 m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;86 m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);87 m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;88 m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;89 m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;90 m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);91 }92 93 /**94 * creates a rotation from an axis and an angle (radians!)95 * @param axis: the rotational axis96 * @param angle: the angle in radians97 */98 Rotation::Rotation (const Vector& axis, float angle)99 {100 float ca, sa;101 ca = cos(angle);102 sa = sin(angle);103 m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);104 m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;105 m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;106 m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;107 m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);108 m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;109 m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;110 m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;111 m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);112 }113 114 /**115 * creates a rotation from euler angles (pitch/yaw/roll)116 * @param pitch: rotation around z (in radians)117 * @param yaw: rotation around y (in radians)118 * @param roll: rotation around x (in radians)119 */120 Rotation::Rotation ( float pitch, float yaw, float roll)121 {122 float cy, sy, cr, sr, cp, sp;123 cy = cos(yaw);124 sy = sin(yaw);125 cr = cos(roll);126 sr = sin(roll);127 cp = cos(pitch);128 sp = sin(pitch);129 m[0] = cy*cr;130 m[1] = -cy*sr;131 m[2] = sy;132 m[3] = cp*sr+sp*sy*cr;133 m[4] = cp*cr-sp*sr*sy;134 m[5] = -sp*cy;135 m[6] = sp*sr-cp*sy*cr;136 m[7] = sp*cr+cp*sy*sr;137 m[8] = cp*cy;138 }139 140 /**141 * creates a nullrotation (an identity rotation)142 */143 Rotation::Rotation ()144 {145 m[0] = 1.0f;146 m[1] = 0.0f;147 m[2] = 0.0f;148 m[3] = 0.0f;149 m[4] = 1.0f;150 m[5] = 0.0f;151 m[6] = 0.0f;152 m[7] = 0.0f;153 m[8] = 1.0f;154 }155 156 /**157 * fills the specified buffer with a 4x4 glmatrix158 * @param buffer: Pointer to an array of 16 floats159 160 Use this to get the rotation in a gl-compatible format161 */162 void Rotation::glmatrix (float* buffer)163 {164 buffer[0] = m[0];165 buffer[1] = m[3];166 buffer[2] = m[6];167 buffer[3] = m[0];168 buffer[4] = m[1];169 buffer[5] = m[4];170 buffer[6] = m[7];171 buffer[7] = m[0];172 buffer[8] = m[2];173 buffer[9] = m[5];174 buffer[10] = m[8];175 buffer[11] = m[0];176 buffer[12] = m[0];177 buffer[13] = m[0];178 buffer[14] = m[0];179 buffer[15] = m[1];180 }181 182 /**183 * multiplies two rotational matrices184 * @param r: another Rotation185 * @return the matrix product of the Rotations186 187 Use this to rotate one rotation by another188 */189 Rotation Rotation::operator* (const Rotation& r)190 {191 Rotation p;192 193 p.m[0] = m[0]*r.m[0] + m[1]*r.m[3] + m[2]*r.m[6];194 p.m[1] = m[0]*r.m[1] + m[1]*r.m[4] + m[2]*r.m[7];195 p.m[2] = m[0]*r.m[2] + m[1]*r.m[5] + m[2]*r.m[8];196 197 p.m[3] = m[3]*r.m[0] + m[4]*r.m[3] + m[5]*r.m[6];198 p.m[4] = m[3]*r.m[1] + m[4]*r.m[4] + m[5]*r.m[7];199 p.m[5] = m[3]*r.m[2] + m[4]*r.m[5] + m[5]*r.m[8];200 201 p.m[6] = m[6]*r.m[0] + m[7]*r.m[3] + m[8]*r.m[6];202 p.m[7] = m[6]*r.m[1] + m[7]*r.m[4] + m[8]*r.m[7];203 p.m[8] = m[6]*r.m[2] + m[7]*r.m[5] + m[8]*r.m[8];204 205 return p;206 }207 208 209 /**210 * rotates the vector by the given rotation211 * @param v: a vector212 * @param r: a rotation213 * @return the rotated vector214 */215 Vector rotateVector( const Vector& v, const Rotation& r)216 {217 Vector t;218 219 t.x = v.x * r.m[0] + v.y * r.m[1] + v.z * r.m[2];220 t.y = v.x * r.m[3] + v.y * r.m[4] + v.z * r.m[5];221 t.z = v.x * r.m[6] + v.y * r.m[7] + v.z * r.m[8];222 223 return t;224 }225 29 226 30 /** … … 299 103 a = t - r; 300 104 } 301 302 /**303 * create a plane from three points304 * @param a: first point305 * @param b: second point306 * @param c: third point307 */308 Plane::Plane (const Vector& a, const Vector& b, const Vector& c)309 {310 n = (a-b).cross(c-b);311 k = -(n.x*b.x+n.y*b.y+n.z*b.z);312 }313 314 /**315 * create a plane from anchor point and normal316 * @param norm: normal vector317 * @param p: anchor point318 */319 Plane::Plane (const Vector& norm, const Vector& p)320 {321 n = norm;322 k = -(n.x*p.x+n.y*p.y+n.z*p.z);323 }324 325 326 /**327 * create a plane from anchor point and normal328 * @param norm: normal vector329 * @param p: anchor point330 */331 Plane::Plane (const Vector& norm, const sVec3D& g)332 {333 Vector p(g[0], g[1], g[2]);334 n = norm;335 k = -(n.x*p.x+n.y*p.y+n.z*p.z);336 }337 338 339 /**340 * returns the intersection point between the plane and a line341 * @param l: a line342 */343 Vector Plane::intersectLine (const Line& l) const344 {345 if (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z == 0.0) return Vector(0,0,0);346 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);347 return l.r + (l.a * t);348 }349 350 /**351 * returns the distance between the plane and a point352 * @param p: a Point353 * @return the distance between the plane and the point (can be negative)354 */355 float Plane::distancePoint (const Vector& p) const356 {357 float l = n.len();358 if( l == 0.0) return 0.0;359 return (n.dot(p) + k) / n.len();360 }361 362 363 /**364 * returns the distance between the plane and a point365 * @param p: a Point366 * @return the distance between the plane and the point (can be negative)367 */368 float Plane::distancePoint (const sVec3D& p) const369 {370 Vector s(p[0], p[1], p[2]);371 float l = n.len();372 if( l == 0.0) return 0.0;373 return (n.dot(s) + k) / n.len();374 }375 376 377 /**378 * returns the side a point is located relative to a Plane379 * @param p: a Point380 * @return 0 if the point is contained within the Plane, positive(negative) if the point is in the positive(negative) semi-space of the Plane381 */382 float Plane::locatePoint (const Vector& p) const383 {384 return (n.dot(p) + k);385 }386
Note: See TracChangeset
for help on using the changeset viewer.