3D数学 RotationMatrix
2015-09-22 19:28
190 查看
1. 如何使用矩阵表示方位
通过列出从一个坐标系到另一个坐标系的转变矩阵来表示两个坐标系之间的关系。例如:从物体坐标系到惯性坐标系的转变矩阵,也可以通过逆矩阵从惯性坐标系转换回物体坐标系。
示例:
我们会看一下如何用一个矩阵将一个点从一个坐标系转变到另一个坐标系。
下图使用一个矩阵将飞机从物体坐标系转换到惯性坐标系。
我们用粗黑箭头指示矩阵的每一行和物体坐标轴的关系。
这个旋转矩阵包含了在惯性坐标系的物体轴。
同时,它是个旋转矩阵。可以用多个行向量跟矩阵相乘,将多个向量从物体坐标系转换到惯性坐标系。
因为旋转矩阵是正交的,因此如果我们想从惯性坐标系转换回物体坐标系,可以使用矩阵的转置(等同于矩阵的逆)作为转换矩阵。
但是,使用矩阵来表示方位有一个问题:不便于程序员阅读和检查代码问题。
2. C++代码实现
///////////////////////////////////////////////////////////////////////////// // // 3D Math Primer for Games and Graphics Development // // Vector3.h - Declarations for 3D vector class // // Visit gamemath.com for the latest version of this file. // // For additional comments, see Chapter 6. // ///////////////////////////////////////////////////////////////////////////// #ifndef __VECTOR3_H_INCLUDED__ #define __VECTOR3_H_INCLUDED__ #include <math.h> ///////////////////////////////////////////////////////////////////////////// // // class Vector3 - a simple 3D vector class // ///////////////////////////////////////////////////////////////////////////// class Vector3 { public: // Public representation: Not many options here. float x,y,z; // Constructors // Default constructor leaves vector in // an indeterminate state Vector3() {} // Copy constructor Vector3(const Vector3 &a) : x(a.x), y(a.y), z(a.z) {} // Construct given three values Vector3(float nx, float ny, float nz) : x(nx), y(ny), z(nz) {} // Standard object maintenance // Assignment. We adhere to C convention and // return reference to the lvalue Vector3 &operator =(const Vector3 &a) { x = a.x; y = a.y; z = a.z; return *this; } // Check for equality bool operator ==(const Vector3 &a) const { return x==a.x && y==a.y && z==a.z; } bool operator !=(const Vector3 &a) const { return x!=a.x || y!=a.y || z!=a.z; } // Vector operations // Set the vector to zero void zero() { x = y = z = 0.0f; } // Unary minus returns the negative of the vector Vector3 operator -() const { return Vector3(-x,-y,-z); } // Binary + and - add and subtract vectors Vector3 operator +(const Vector3 &a) const { return Vector3(x + a.x, y + a.y, z + a.z); } Vector3 operator -(const Vector3 &a) const { return Vector3(x - a.x, y - a.y, z - a.z); } // Multiplication and division by scalar Vector3 operator *(float a) const { return Vector3(x*a, y*a, z*a); } Vector3 operator /(float a) const { float oneOverA = 1.0f / a; // NOTE: no check for divide by zero here return Vector3(x*oneOverA, y*oneOverA, z*oneOverA); } // Combined assignment operators to conform to // C notation convention Vector3 &operator +=(const Vector3 &a) { x += a.x; y += a.y; z += a.z; return *this; } Vector3 &operator -=(const Vector3 &a) { x -= a.x; y -= a.y; z -= a.z; return *this; } Vector3 &operator *=(float a) { x *= a; y *= a; z *= a; return *this; } Vector3 &operator /=(float a) { float oneOverA = 1.0f / a; x *= oneOverA; y *= oneOverA; z *= oneOverA; return *this; } // Normalize the vector void normalize() { float magSq = x*x + y*y + z*z; if (magSq > 0.0f) { // check for divide-by-zero float oneOverMag = 1.0f / sqrt(magSq); x *= oneOverMag; y *= oneOverMag; z *= oneOverMag; } } // Vector dot product. We overload the standard // multiplication symbol to do this float operator *(const Vector3 &a) const { return x*a.x + y*a.y + z*a.z; } }; ///////////////////////////////////////////////////////////////////////////// // // Nonmember functions // ///////////////////////////////////////////////////////////////////////////// // Compute the magnitude of a vector inline float vectorMag(const Vector3 &a) { return sqrt(a.x*a.x + a.y*a.y + a.z*a.z); } // Compute the cross product of two vectors inline Vector3 crossProduct(const Vector3 &a, const Vector3 &b) { return Vector3( a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x ); } // Scalar on the left multiplication, for symmetry inline Vector3 operator *(float k, const Vector3 &v) { return Vector3(k*v.x, k*v.y, k*v.z); } // Compute the distance between two points inline float distance(const Vector3 &a, const Vector3 &b) { float dx = a.x - b.x; float dy = a.y - b.y; float dz = a.z - b.z; return sqrt(dx*dx + dy*dy + dz*dz); } // Compute the distance between two points, squared. Often useful // when comparing distances, since the square root is slow inline float distanceSquared(const Vector3 &a, const Vector3 &b) { float dx = a.x - b.x; float dy = a.y - b.y; float dz = a.z - b.z; return dx*dx + dy*dy + dz*dz; } ///////////////////////////////////////////////////////////////////////////// // // Global variables // ///////////////////////////////////////////////////////////////////////////// // We provide a global zero vector constant extern const Vector3 kZeroVector; ///////////////////////////////////////////////////////////////////////////// #endif // #ifndef __VECTOR3_H_INCLUDED__
///////////////////////////////////////////////////////////////////////////// // // 3D Math Primer for Games and Graphics Development // // RotationMatrix.h - Declarations for class RotationMatrix // // Visit gamemath.com for the latest version of this file. // // For more details, see RotationMatrix.cpp // ///////////////////////////////////////////////////////////////////////////// #ifndef __ROTATIONMATRIX_H_INCLUDED__ #define __ROTATIONMATRIX_H_INCLUDED__ class Vector3; class EulerAngles; class Quaternion; //--------------------------------------------------------------------------- // class RotationMatrix // // Implement a simple 3x3 matrix that is used for ROTATION ONLY. The // matrix is assumed to be orthogonal. The direction of transformation // is specified at the time of transformation. class RotationMatrix { public: // Public data // The 9 values of the matrix. See RotationMatrix.cpp file for // the details of the layout float m11, m12, m13; float m21, m22, m23; float m31, m32, m33; // Public operations // Set to identity void identity(); // Setup the matrix with a specified orientation void setup(const EulerAngles &orientation); // Setup the matrix from a quaternion, assuming the // quaternion performs the rotation in the // specified direction of transformation void fromInertialToObjectQuaternion(const Quaternion &q); void fromObjectToInertialQuaternion(const Quaternion &q); // Perform rotations Vector3 inertialToObject(const Vector3 &v) const; Vector3 objectToInertial(const Vector3 &v) const; }; ///////////////////////////////////////////////////////////////////////////// #endif // #ifndef __ROTATIONMATRIX_H_INCLUDED__
///////////////////////////////////////////////////////////////////////////// // // 3D Math Primer for Games and Graphics Development // // RotationMatrix.cpp - Implementation of class RotationMatrix // // Visit gamemath.com for the latest version of this file. // // For more details see section 11.4. // ///////////////////////////////////////////////////////////////////////////// #include "vector3.h" #include "RotationMatrix.h" #include "MathUtil.h" #include "Quaternion.h" #include "EulerAngles.h" ///////////////////////////////////////////////////////////////////////////// // // class RotationMatrix // //--------------------------------------------------------------------------- // // MATRIX ORGANIZATION // // A user of this class should rarely care how the matrix is organized. // However, it is of course important that internally we keep everything // straight. // // The matrix is assumed to be a rotation matrix only, and therefore // orthoganal. The "forward" direction of transformation (if that really // even applies in this case) will be from inertial to object space. // To perform an object->inertial rotation, we will multiply by the // transpose. // // In other words: // // Inertial to object: // // | m11 m12 m13 | // [ ix iy iz ] | m21 m22 m23 | = [ ox oy oz ] // | m31 m32 m33 | // // Object to inertial: // // | m11 m21 m31 | // [ ox oy oz ] | m12 m22 m32 | = [ ix iy iz ] // | m13 m23 m33 | // // Or, using column vector notation: // // Inertial to object: // // | m11 m21 m31 | | ix | | ox | // | m12 m22 m32 | | iy | = | oy | // | m13 m23 m33 | | iz | | oz | // // Object to inertial: // // | m11 m12 m13 | | ox | | ix | // | m21 m22 m23 | | oy | = | iy | // | m31 m32 m33 | | oz | | iz | // ///////////////////////////////////////////////////////////////////////////// //--------------------------------------------------------------------------- // RotationMatrix::identity // // Set the matrix to the identity matrix void RotationMatrix::identity() { m11 = 1.0f; m12 = 0.0f; m13 = 0.0f; m21 = 0.0f; m22 = 1.0f; m23 = 0.0f; m31 = 0.0f; m32 = 0.0f; m33 = 1.0f; } //--------------------------------------------------------------------------- // RotationMatrix::setup // // Setup the matrix with the specified orientation // // See 10.6.1 void RotationMatrix::setup(const EulerAngles &orientation) { // Fetch sine and cosine of angles float sh,ch, sp,cp, sb,cb; sinCos(&sh, &ch, orientation.heading); sinCos(&sp, &cp, orientation.pitch); sinCos(&sb, &cb, orientation.bank); // Fill in the matrix elements m11 = ch * cb + sh * sp * sb; m12 = -ch * sb + sh * sp * cb; m13 = sh * cp; m21 = sb * cp; m22 = cb * cp; m23 = -sp; m31 = -sh * cb + ch * sp * sb; m32 = sb * sh + ch * sp * cb; m33 = ch * cp; } //--------------------------------------------------------------------------- // RotationMatrix::fromInertialToObjectQuaternion // // Setup the matrix, given a quaternion that performs an inertial->object // rotation // // See 10.6.3 void RotationMatrix::fromInertialToObjectQuaternion(const Quaternion &q) { // Fill in the matrix elements. This could possibly be // optimized since there are many common subexpressions. // We'll leave that up to the compiler... m11 = 1.0f - 2.0f * (q.y*q.y + q.z*q.z); m12 = 2.0f * (q.x*q.y + q.w*q.z); m13 = 2.0f * (q.x*q.z - q.w*q.y); m21 = 2.0f * (q.x*q.y - q.w*q.z); m22 = 1.0f - 2.0f * (q.x*q.x + q.z*q.z); m23 = 2.0f * (q.y*q.z + q.w*q.x); m31 = 2.0f * (q.x*q.z + q.w*q.y); m32 = 2.0f * (q.y*q.z - q.w*q.x); m33 = 1.0f - 2.0f * (q.x*q.x + q.y*q.y); } //--------------------------------------------------------------------------- // RotationMatrix::fromObjectToInertialQuaternion // // Setup the matrix, given a quaternion that performs an object->inertial // rotation // // See 10.6.3 void RotationMatrix::fromObjectToInertialQuaternion(const Quaternion &q) { // Fill in the matrix elements. This could possibly be // optimized since there are many common subexpressions. // We'll leave that up to the compiler... m11 = 1.0f - 2.0f * (q.y*q.y + q.z*q.z); m12 = 2.0f * (q.x*q.y - q.w*q.z); m13 = 2.0f * (q.x*q.z + q.w*q.y); m21 = 2.0f * (q.x*q.y + q.w*q.z); m22 = 1.0f - 2.0f * (q.x*q.x + q.z*q.z); m23 = 2.0f * (q.y*q.z - q.w*q.x); m31 = 2.0f * (q.x*q.z - q.w*q.y); m32 = 2.0f * (q.y*q.z + q.w*q.x); m33 = 1.0f - 2.0f * (q.x*q.x + q.y*q.y); } //--------------------------------------------------------------------------- // RotationMatrix::inertialToObject // // Rotate a vector from inertial to object space Vector3 RotationMatrix::inertialToObject(const Vector3 &v) const { // Perform the matrix multiplication in the "standard" way. return Vector3( m11*v.x + m21*v.y + m31*v.z, m12*v.x + m22*v.y + m32*v.z, m13*v.x + m23*v.y + m33*v.z ); } //--------------------------------------------------------------------------- // RotationMatrix::objectToInertial // // Rotate a vector from object to inertial space Vector3 RotationMatrix::objectToInertial(const Vector3 &v) const { // Multiply by the transpose return Vector3( m11*v.x + m12*v.y + m13*v.z, m21*v.x + m22*v.y + m23*v.z, m31*v.x + m32*v.y + m33*v.z ); }
#include "RotationMatrix.h" #include <iostream> using namespace std; void prfloatVec(Vector3 &vec) { cout << "[" << vec.x << "," << vec.y << "," << vec.z << "]" << endl; } int main() { cout << "hello 矩阵的逆" << endl; RotationMatrix m; m.m11 = 0.866f; m.m12 = 0.0f; m.m13 = -0.5f; m.m21 = 0.0f; m.m22 = 1.0f; m.m23 = 0.0f; m.m31 = 0.5f; m.m32 = 0.0f; m.m33 = 0.866f; Vector3 v(10,20,30); Vector3 v2; v2 = m.inertialToObject(v); prfloatVec(v2); v2 = m.objectToInertial(v); prfloatVec(v2); }
3. 程序运行结果
hello 矩阵的逆[23.66,20,20.98]
[-6.34,20,30.98]
相关文章推荐
- Networking - ICMP 协议
- 感想
- 用MPMoviePlayerController播放视频的方法
- 我终于成为了BZOJ权限狗
- GOF 23 设计模式之 原型模式(prototype) 深复制与浅复制举例
- Repository模式
- Repository模式
- MWeb 1.7.1 版发布!支持导出为 RTF 和 Docx、发布到 Evernote 带样式、文档库备份和新网站主题等大量改进!
- 屏幕录制
- HDOJ 2057 A + B Again (十六进制)
- Python同步数据库的数据到Neo4J
- eclipse 利用已有c++代码建工程,并编译执行
- JAVA上机——3.4
- CopyOnWriteArrayList与Collections.synchronizedMap性能比较
- BZOJ 1801 [Ahoi2009]chess 中国象棋 递推
- Linux自动重启was
- web+birt
- unix环境编程学习笔记------套接字缓冲区的原理
- 关于IO的6个课上小实验
- Web APi之HttpClient注意事项以及建议(四)