fMatrix4.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00009 /*
00010  * fMatrix4.cpp
00011  * Create: Katsu Yamane, Univ. of Tokyo, 03.06.17
00012  */
00013 
00014 #include "fMatrix4.h"
00015 
00016 void fVec4::cross(const fVec4& vec1, const fVec4& vec2)
00017 {
00018         m_scalar = vec1.m_scalar*vec2.m_scalar - vec1.m_vec*vec2.m_vec;
00019         m_vec.cross(vec1.m_vec, vec2.m_vec);
00020         m_vec += vec1.m_scalar*vec2.m_vec;
00021         m_vec += vec2.m_scalar*vec1.m_vec;
00022 }
00023 
00024 ostream& operator << (ostream& ost, const fVec4& mat)
00025 {
00026         ost << "(" << mat.m_scalar << " [" << mat.m_vec(0) << " " << mat.m_vec(1) << " " << mat.m_vec(2) << "])" << flush;
00027         return ost;
00028 }
00029 
00030 double& fMat44::operator () (int i, int j)
00031 {
00032 #ifndef NDEBUG
00033         if(i < 0 || i >= 4 || j < 0 || j >= 4)
00034         {
00035                 cerr << "matrix size error at operator ()" << endl;
00036                 return temp;
00037         }
00038 #endif
00039         if(i<3 && j<3) return m_mat(i,j);
00040         else if(i<3 && j==3) return m_vec(i);
00041         else if(i==3 && j==3) return m_scalar;
00042         else return temp;
00043 }
00044 
00045 double fMat44::operator () (int i, int j) const
00046 {
00047 #ifndef NDEBUG
00048         if(i < 0 || i >= 4 || j < 0 || j >= 4)
00049         {
00050                 cerr << "matrix size error at operator ()" << endl;
00051                 return temp;
00052         }
00053 #endif
00054         if(i<3 && j<3) return m_mat(i,j);
00055         else if(i<3 && j==3) return m_vec(i);
00056         else if(i==3 && j==3) return m_scalar;
00057         else return temp;
00058 }
00059 
00060 void fMat44::operator = (double d)
00061 {
00062         m_mat = d;
00063         m_vec = d;
00064         m_scalar = d;
00065 }
00066 
00067 fMat44 fMat44::operator = (const fMat44& mat)
00068 {
00069         m_mat = mat.m_mat;
00070         m_vec = mat.m_vec;
00071         m_scalar = mat.m_scalar;
00072         return *this;
00073 }
00074 
00075 void fMat44::operator += (const fMat44& m)
00076 {
00077         m_mat += m.m_mat;
00078         m_vec += m.m_vec;
00079         m_scalar += m.m_scalar;
00080 }
00081 
00082 void fMat44::operator -= (const fMat44& m)
00083 {
00084         m_mat -= m.m_mat;
00085         m_vec -= m.m_vec;
00086         m_scalar -= m.m_scalar;
00087 }
00088 
00089 void fMat44::operator *= (double d)
00090 {
00091         m_mat *= d;
00092         m_vec *= d;
00093         m_scalar *= d;
00094 }
00095 
00096 void fMat44::operator /= (double d)
00097 {
00098         m_mat /= d;
00099         m_vec /= d;
00100         m_scalar /= d;
00101 }
00102 
00103 fMat44 operator - (const fMat44& m)
00104 {
00105         fMat44 ret;
00106         ret.m_mat = - m.m_mat;
00107         ret.m_vec = - m.m_vec;
00108         ret.m_scalar = - m.m_scalar;
00109         return ret;
00110 }
00111 
00112 fMat44 operator * (const fMat44& m1, const fMat44& m2)
00113 {
00114         fMat44 ret;
00115         ret.m_mat = m1.m_mat * m2.m_mat;
00116         ret.m_vec = m1.m_mat * m2.m_vec + m1.m_vec * m2.m_scalar;
00117         ret.m_scalar = m1.m_scalar * m2.m_scalar;
00118         return ret;
00119 }
00120 
00121 fMat44 operator + (const fMat44& m1, const fMat44& m2)
00122 {
00123         fMat44 ret;
00124         ret.m_mat = m1.m_mat + m2.m_mat;
00125         ret.m_vec = m1.m_vec + m2.m_vec;
00126         ret.m_scalar = m1.m_scalar + m2.m_scalar;
00127         return ret;
00128 }
00129 
00130 fMat44 operator - (const fMat44& m1, const fMat44& m2)
00131 {
00132         fMat44 ret;
00133         ret.m_mat = m1.m_mat - m2.m_mat;
00134         ret.m_vec = m1.m_vec - m2.m_vec;
00135         ret.m_scalar = m1.m_scalar - m2.m_scalar;
00136         return ret;
00137 }
00138 
00139 fVec4 operator * (const fMat44& m, const fVec4& v)
00140 {
00141         fVec4 ret, tmp(v);
00142         ret.Vec() = m.m_mat * tmp.Vec() + m.m_vec * tmp.Scalar();
00143         ret.Scalar() = m.m_scalar * tmp.Scalar();
00144         return ret;
00145 }
00146 
00147 /*
00148  * fVec4
00149  */
00150 void fVec4::sub(const fVec4& vec1, const fVec4& vec2)
00151 {
00152         m_scalar = vec1.m_scalar - vec2.m_scalar;
00153         m_vec.sub(vec1.m_vec, vec2.m_vec);
00154 }
00155 
00156 void fVec4::add(const fVec4& vec1, const fVec4& vec2)
00157 {
00158         m_scalar = vec1.m_scalar + vec2.m_scalar;
00159         m_vec.add(vec1.m_vec, vec2.m_vec);
00160 }
00161 
00162 void fVec4::mul(fVec4& _vec, double d)
00163 {
00164         m_vec.mul(_vec.m_vec, d);
00165         m_scalar = _vec.m_scalar * d;
00166 }
00167 
00168 double& fVec4::operator () (int i)
00169 {
00170 #ifndef NDEBUG
00171         if(i < 0 || i >= 4)
00172         {
00173                 cerr << "vector size error at operator ()" << endl;
00174                 return temp;
00175         }
00176 #endif
00177         if(i<3) return m_vec(i);
00178         else if(i==3) return m_scalar;
00179         else return temp;
00180 }
00181 
00182 double fVec4::operator () (int i) const
00183 {
00184 #ifndef NDEBUG
00185         if(i < 0 || i >= 4)
00186         {
00187                 cerr << "vector size error at operator ()" << endl;
00188                 return temp;
00189         }
00190 #endif
00191         if(i<3) return m_vec(i);
00192         else if(i==3) return m_scalar;
00193         else return temp;
00194 }
00195 
00196 fVec4 fVec4::operator = (const fVec4& vec)
00197 {
00198         m_vec = vec.m_vec;
00199         m_scalar = vec.m_scalar;
00200         return (*this);
00201 }
00202 
00203 void fVec4::operator = (double d)
00204 {
00205         int i;
00206         for(i=0; i<4; i++) (*this)(i) = d;
00207 }
00208 
00209 void fVec4::operator += (const fVec4& vec)
00210 {
00211         m_vec += vec.m_vec;
00212         m_scalar += vec.m_scalar;
00213 }
00214 
00215 void fVec4::operator -= (const fVec4& vec)
00216 {
00217         m_vec -= vec.m_vec;
00218         m_scalar -= vec.m_scalar;
00219 }
00220 
00221 void fVec4::operator *= (double d)
00222 {
00223         m_vec *= d;
00224         m_scalar *= d;
00225 }
00226 
00227 void fVec4::operator /= (double d)
00228 {
00229         m_vec /= d;
00230         m_scalar /= d;
00231 }
00232 
00233 fVec4 operator - (const fVec4& vec)
00234 {
00235         fVec4 ret;
00236         ret.m_vec -= vec.m_vec;
00237         ret.m_scalar -= vec.m_scalar;
00238         return ret;
00239 }
00240 
00241 double operator * (const fVec4& v1, const fVec4& v2)
00242 {
00243         return (v1.m_vec * v2.m_vec + v1.m_scalar * v2.m_scalar);
00244 }
00245 
00246 fVec4 operator * (double d, const fVec4& v)
00247 {
00248         fVec4 ret;
00249         ret.m_vec = d * v.m_vec;
00250         ret.m_scalar = d * v.m_scalar;
00251         return ret;
00252 }
00253 
00254 fVec4 operator + (const fVec4& v1, const fVec4& v2)
00255 {
00256         fVec4 ret;
00257         ret.m_vec = v1.m_vec + v2.m_vec;
00258         ret.m_scalar = v1.m_scalar + v2.m_scalar;
00259         return ret;
00260 }
00261 
00262 fVec4 operator - (const fVec4& v1, const fVec4& v2)
00263 {
00264         fVec4 ret;
00265         ret.m_vec = v1.m_vec - v2.m_vec;
00266         ret.m_scalar = v1.m_scalar - v2.m_scalar;
00267         return ret;
00268 }
00269 
00270 void fVec4::zero()
00271 {
00272         m_vec.zero();
00273         m_scalar = 0;
00274 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53