/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /* * vector3.cpp * Copyright (C) Andrew Tridgell 2012 * * This file 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 3 of the License, or * (at your option) any later version. * * This file 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, see . */ #include "AP_Math.h" #define HALF_SQRT_2 0.70710678118654757f // rotate a vector by a standard rotation, attempting // to use the minimum number of floating point operations template void Vector3::rotate(enum Rotation rotation) { T tmp; switch (rotation) { case ROTATION_NONE: case ROTATION_MAX: return; case ROTATION_YAW_45: { tmp = HALF_SQRT_2*(x - y); y = HALF_SQRT_2*(x + y); x = tmp; return; } case ROTATION_YAW_90: { tmp = x; x = -y; y = tmp; return; } case ROTATION_YAW_135: { tmp = -HALF_SQRT_2*(x + y); y = HALF_SQRT_2*(x - y); x = tmp; return; } case ROTATION_YAW_180: x = -x; y = -y; return; case ROTATION_YAW_225: { tmp = HALF_SQRT_2*(y - x); y = -HALF_SQRT_2*(x + y); x = tmp; return; } case ROTATION_YAW_270: { tmp = x; x = y; y = -tmp; return; } case ROTATION_YAW_315: { tmp = HALF_SQRT_2*(x + y); y = HALF_SQRT_2*(y - x); x = tmp; return; } case ROTATION_ROLL_180: { y = -y; z = -z; return; } case ROTATION_ROLL_180_YAW_45: { tmp = HALF_SQRT_2*(x + y); y = HALF_SQRT_2*(x - y); x = tmp; z = -z; return; } case ROTATION_ROLL_180_YAW_90: { tmp = x; x = y; y = tmp; z = -z; return; } case ROTATION_ROLL_180_YAW_135: { tmp = HALF_SQRT_2*(y - x); y = HALF_SQRT_2*(y + x); x = tmp; z = -z; return; } case ROTATION_PITCH_180: { x = -x; z = -z; return; } case ROTATION_ROLL_180_YAW_225: { tmp = -HALF_SQRT_2*(x + y); y = HALF_SQRT_2*(y - x); x = tmp; z = -z; return; } case ROTATION_ROLL_180_YAW_270: { tmp = x; x = -y; y = -tmp; z = -z; return; } case ROTATION_ROLL_180_YAW_315: { tmp = HALF_SQRT_2*(x - y); y = -HALF_SQRT_2*(x + y); x = tmp; z = -z; return; } case ROTATION_ROLL_90: { tmp = z; z = y; y = -tmp; return; } case ROTATION_ROLL_90_YAW_45: { tmp = z; z = y; y = -tmp; tmp = HALF_SQRT_2*(x - y); y = HALF_SQRT_2*(x + y); x = tmp; return; } case ROTATION_ROLL_90_YAW_90: { tmp = z; z = y; y = -tmp; tmp = x; x = -y; y = tmp; return; } case ROTATION_ROLL_90_YAW_135: { tmp = z; z = y; y = -tmp; tmp = -HALF_SQRT_2*(x + y); y = HALF_SQRT_2*(x - y); x = tmp; return; } case ROTATION_ROLL_270: { tmp = z; z = -y; y = tmp; return; } case ROTATION_ROLL_270_YAW_45: { tmp = z; z = -y; y = tmp; tmp = HALF_SQRT_2*(x - y); y = HALF_SQRT_2*(x + y); x = tmp; return; } case ROTATION_ROLL_270_YAW_90: { tmp = z; z = -y; y = tmp; tmp = x; x = -y; y = tmp; return; } case ROTATION_ROLL_270_YAW_135: { tmp = z; z = -y; y = tmp; tmp = -HALF_SQRT_2*(x + y); y = HALF_SQRT_2*(x - y); x = tmp; return; } case ROTATION_PITCH_90: { tmp = z; z = -x; x = tmp; return; } case ROTATION_PITCH_270: { tmp = z; z = x; x = -tmp; return; } case ROTATION_PITCH_180_YAW_90: { z = -z; tmp = -x; x = -y; y = tmp; return; } case ROTATION_PITCH_180_YAW_270: { x = -x; z = -z; tmp = x; x = y; y = -tmp; return; } case ROTATION_ROLL_90_PITCH_90: { tmp = z; z = y; y = -tmp; tmp = z; z = -x; x = tmp; return; } case ROTATION_ROLL_180_PITCH_90: { y = -y; z = -z; tmp = z; z = -x; x = tmp; return; } case ROTATION_ROLL_270_PITCH_90: { tmp = z; z = -y; y = tmp; tmp = z; z = -x; x = tmp; return; } case ROTATION_ROLL_90_PITCH_180: { tmp = z; z = y; y = -tmp; x = -x; z = -z; return; } case ROTATION_ROLL_270_PITCH_180: { tmp = z; z = -y; y = tmp; x = -x; z = -z; return; } case ROTATION_ROLL_90_PITCH_270: { tmp = z; z = y; y = -tmp; tmp = z; z = x; x = -tmp; return; } case ROTATION_ROLL_180_PITCH_270: { y = -y; z = -z; tmp = z; z = x; x = -tmp; return; } case ROTATION_ROLL_270_PITCH_270: { tmp = z; z = -y; y = tmp; tmp = z; z = x; x = -tmp; return; } case ROTATION_ROLL_90_PITCH_180_YAW_90: { tmp = z; z = y; y = -tmp; x = -x; z = -z; tmp = x; x = -y; y = tmp; return; } case ROTATION_ROLL_90_YAW_270: { tmp = z; z = y; y = -tmp; tmp = x; x = y; y = -tmp; return; } case ROTATION_YAW_293_PITCH_68_ROLL_180: { float tmpx = x; float tmpy = y; float tmpz = z; x = 0.1430389f * tmpx -0.9184465f * tmpy -0.3687762f * tmpz; y = -0.3321327f * tmpx -0.3955452f * tmpy +0.8562895f * tmpz; z = -0.9323238f * tmpx -0.00000003f * tmpy -0.3616245f * tmpz; return; } } } // vector cross product template Vector3 Vector3::operator %(const Vector3 &v) const { Vector3 temp(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x); return temp; } // dot product template T Vector3::operator *(const Vector3 &v) const { return x*v.x + y*v.y + z*v.z; } template float Vector3::length(void) const { return pythagorous3(x, y, z); } template Vector3 &Vector3::operator *=(const T num) { x*=num; y*=num; z*=num; return *this; } template Vector3 &Vector3::operator /=(const T num) { x /= num; y /= num; z /= num; return *this; } template Vector3 &Vector3::operator -=(const Vector3 &v) { x -= v.x; y -= v.y; z -= v.z; return *this; } template bool Vector3::is_nan(void) const { return isnan(x) || isnan(y) || isnan(z); } template bool Vector3::is_inf(void) const { return isinf(x) || isinf(y) || isinf(z); } template Vector3 &Vector3::operator +=(const Vector3 &v) { x+=v.x; y+=v.y; z+=v.z; return *this; } template Vector3 Vector3::operator /(const T num) const { return Vector3(x/num, y/num, z/num); } template Vector3 Vector3::operator *(const T num) const { return Vector3(x*num, y*num, z*num); } template Vector3 Vector3::operator -(const Vector3 &v) const { return Vector3(x-v.x, y-v.y, z-v.z); } template Vector3 Vector3::operator +(const Vector3 &v) const { return Vector3(x+v.x, y+v.y, z+v.z); } template Vector3 Vector3::operator -(void) const { return Vector3(-x,-y,-z); } template bool Vector3::operator ==(const Vector3 &v) const { return (x==v.x && y==v.y && z==v.z); } template bool Vector3::operator !=(const Vector3 &v) const { return (x!=v.x && y!=v.y && z!=v.z); } template float Vector3::angle(const Vector3 &v2) const { return acosf(((*this)*v2) / (this->length()*v2.length())); } // multiplication of transpose by a vector template Vector3 Vector3::operator *(const Matrix3 &m) const { return Vector3(*this * m.colx(), *this * m.coly(), *this * m.colz()); } // multiply a column vector by a row vector, returning a 3x3 matrix template Matrix3 Vector3::mul_rowcol(const Vector3 &v2) const { const Vector3 v1 = *this; return Matrix3(v1.x * v2.x, v1.x * v2.y, v1.x * v2.z, v1.y * v2.x, v1.y * v2.y, v1.y * v2.z, v1.z * v2.x, v1.z * v2.y, v1.z * v2.z); } // only define for float template void Vector3::rotate(enum Rotation); template float Vector3::length(void) const; template Vector3 Vector3::operator %(const Vector3 &v) const; template float Vector3::operator *(const Vector3 &v) const; template Vector3 Vector3::operator *(const Matrix3 &m) const; template Matrix3 Vector3::mul_rowcol(const Vector3 &v) const; template Vector3 &Vector3::operator *=(const float num); template Vector3 &Vector3::operator /=(const float num); template Vector3 &Vector3::operator -=(const Vector3 &v); template Vector3 &Vector3::operator +=(const Vector3 &v); template Vector3 Vector3::operator /(const float num) const; template Vector3 Vector3::operator *(const float num) const; template Vector3 Vector3::operator +(const Vector3 &v) const; template Vector3 Vector3::operator -(const Vector3 &v) const; template Vector3 Vector3::operator -(void) const; template bool Vector3::operator ==(const Vector3 &v) const; template bool Vector3::operator !=(const Vector3 &v) const; template bool Vector3::is_nan(void) const; template bool Vector3::is_inf(void) const; template float Vector3::angle(const Vector3 &v) const; #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 template void Vector3::rotate(enum Rotation); template float Vector3::length(void) const; template Vector3 Vector3::operator %(const Vector3 &v) const; template double Vector3::operator *(const Vector3 &v) const; template Vector3 Vector3::operator *(const Matrix3 &m) const; template Matrix3 Vector3::mul_rowcol(const Vector3 &v) const; template Vector3 &Vector3::operator *=(const double num); template Vector3 &Vector3::operator /=(const double num); template Vector3 &Vector3::operator -=(const Vector3 &v); template Vector3 &Vector3::operator +=(const Vector3 &v); template Vector3 Vector3::operator /(const double num) const; template Vector3 Vector3::operator *(const double num) const; template Vector3 Vector3::operator +(const Vector3 &v) const; template Vector3 Vector3::operator -(const Vector3 &v) const; template Vector3 Vector3::operator -(void) const; template bool Vector3::operator ==(const Vector3 &v) const; template bool Vector3::operator !=(const Vector3 &v) const; template bool Vector3::is_nan(void) const; template bool Vector3::is_inf(void) const; template float Vector3::angle(const Vector3 &v) const; #endif