fMatrix4.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * The University of Tokyo
8  */
9 /*
10  * fMatrix4.cpp
11  * Create: Katsu Yamane, Univ. of Tokyo, 03.06.17
12  */
13 
14 #include "fMatrix4.h"
15 
16 void fVec4::cross(const fVec4& vec1, const fVec4& vec2)
17 {
18  m_scalar = vec1.m_scalar*vec2.m_scalar - vec1.m_vec*vec2.m_vec;
19  m_vec.cross(vec1.m_vec, vec2.m_vec);
20  m_vec += vec1.m_scalar*vec2.m_vec;
21  m_vec += vec2.m_scalar*vec1.m_vec;
22 }
23 
24 ostream& operator << (ostream& ost, const fVec4& mat)
25 {
26  ost << "(" << mat.m_scalar << " [" << mat.m_vec(0) << " " << mat.m_vec(1) << " " << mat.m_vec(2) << "])" << flush;
27  return ost;
28 }
29 
30 double& fMat44::operator () (int i, int j)
31 {
32 #ifndef NDEBUG
33  if(i < 0 || i >= 4 || j < 0 || j >= 4)
34  {
35  cerr << "matrix size error at operator ()" << endl;
36  return temp;
37  }
38 #endif
39  if(i<3 && j<3) return m_mat(i,j);
40  else if(i<3 && j==3) return m_vec(i);
41  else if(i==3 && j==3) return m_scalar;
42  else return temp;
43 }
44 
45 double fMat44::operator () (int i, int j) const
46 {
47 #ifndef NDEBUG
48  if(i < 0 || i >= 4 || j < 0 || j >= 4)
49  {
50  cerr << "matrix size error at operator ()" << endl;
51  return temp;
52  }
53 #endif
54  if(i<3 && j<3) return m_mat(i,j);
55  else if(i<3 && j==3) return m_vec(i);
56  else if(i==3 && j==3) return m_scalar;
57  else return temp;
58 }
59 
60 void fMat44::operator = (double d)
61 {
62  m_mat = d;
63  m_vec = d;
64  m_scalar = d;
65 }
66 
68 {
69  m_mat = mat.m_mat;
70  m_vec = mat.m_vec;
71  m_scalar = mat.m_scalar;
72  return *this;
73 }
74 
76 {
77  m_mat += m.m_mat;
78  m_vec += m.m_vec;
79  m_scalar += m.m_scalar;
80 }
81 
83 {
84  m_mat -= m.m_mat;
85  m_vec -= m.m_vec;
86  m_scalar -= m.m_scalar;
87 }
88 
89 void fMat44::operator *= (double d)
90 {
91  m_mat *= d;
92  m_vec *= d;
93  m_scalar *= d;
94 }
95 
96 void fMat44::operator /= (double d)
97 {
98  m_mat /= d;
99  m_vec /= d;
100  m_scalar /= d;
101 }
102 
104 {
105  fMat44 ret;
106  ret.m_mat = - m.m_mat;
107  ret.m_vec = - m.m_vec;
108  ret.m_scalar = - m.m_scalar;
109  return ret;
110 }
111 
112 fMat44 operator * (const fMat44& m1, const fMat44& m2)
113 {
114  fMat44 ret;
115  ret.m_mat = m1.m_mat * m2.m_mat;
116  ret.m_vec = m1.m_mat * m2.m_vec + m1.m_vec * m2.m_scalar;
117  ret.m_scalar = m1.m_scalar * m2.m_scalar;
118  return ret;
119 }
120 
121 fMat44 operator + (const fMat44& m1, const fMat44& m2)
122 {
123  fMat44 ret;
124  ret.m_mat = m1.m_mat + m2.m_mat;
125  ret.m_vec = m1.m_vec + m2.m_vec;
126  ret.m_scalar = m1.m_scalar + m2.m_scalar;
127  return ret;
128 }
129 
130 fMat44 operator - (const fMat44& m1, const fMat44& m2)
131 {
132  fMat44 ret;
133  ret.m_mat = m1.m_mat - m2.m_mat;
134  ret.m_vec = m1.m_vec - m2.m_vec;
135  ret.m_scalar = m1.m_scalar - m2.m_scalar;
136  return ret;
137 }
138 
139 fVec4 operator * (const fMat44& m, const fVec4& v)
140 {
141  fVec4 ret, tmp(v);
142  ret.Vec() = m.m_mat * tmp.Vec() + m.m_vec * tmp.Scalar();
143  ret.Scalar() = m.m_scalar * tmp.Scalar();
144  return ret;
145 }
146 
147 /*
148  * fVec4
149  */
150 void fVec4::sub(const fVec4& vec1, const fVec4& vec2)
151 {
152  m_scalar = vec1.m_scalar - vec2.m_scalar;
153  m_vec.sub(vec1.m_vec, vec2.m_vec);
154 }
155 
156 void fVec4::add(const fVec4& vec1, const fVec4& vec2)
157 {
158  m_scalar = vec1.m_scalar + vec2.m_scalar;
159  m_vec.add(vec1.m_vec, vec2.m_vec);
160 }
161 
162 void fVec4::mul(fVec4& _vec, double d)
163 {
164  m_vec.mul(_vec.m_vec, d);
165  m_scalar = _vec.m_scalar * d;
166 }
167 
168 double& fVec4::operator () (int i)
169 {
170 #ifndef NDEBUG
171  if(i < 0 || i >= 4)
172  {
173  cerr << "vector size error at operator ()" << endl;
174  return temp;
175  }
176 #endif
177  if(i<3) return m_vec(i);
178  else if(i==3) return m_scalar;
179  else return temp;
180 }
181 
182 double fVec4::operator () (int i) const
183 {
184 #ifndef NDEBUG
185  if(i < 0 || i >= 4)
186  {
187  cerr << "vector size error at operator ()" << endl;
188  return temp;
189  }
190 #endif
191  if(i<3) return m_vec(i);
192  else if(i==3) return m_scalar;
193  else return temp;
194 }
195 
197 {
198  m_vec = vec.m_vec;
199  m_scalar = vec.m_scalar;
200  return (*this);
201 }
202 
203 void fVec4::operator = (double d)
204 {
205  int i;
206  for(i=0; i<4; i++) (*this)(i) = d;
207 }
208 
209 void fVec4::operator += (const fVec4& vec)
210 {
211  m_vec += vec.m_vec;
212  m_scalar += vec.m_scalar;
213 }
214 
215 void fVec4::operator -= (const fVec4& vec)
216 {
217  m_vec -= vec.m_vec;
218  m_scalar -= vec.m_scalar;
219 }
220 
221 void fVec4::operator *= (double d)
222 {
223  m_vec *= d;
224  m_scalar *= d;
225 }
226 
227 void fVec4::operator /= (double d)
228 {
229  m_vec /= d;
230  m_scalar /= d;
231 }
232 
234 {
235  fVec4 ret;
236  ret.m_vec -= vec.m_vec;
237  ret.m_scalar -= vec.m_scalar;
238  return ret;
239 }
240 
241 double operator * (const fVec4& v1, const fVec4& v2)
242 {
243  return (v1.m_vec * v2.m_vec + v1.m_scalar * v2.m_scalar);
244 }
245 
246 fVec4 operator * (double d, const fVec4& v)
247 {
248  fVec4 ret;
249  ret.m_vec = d * v.m_vec;
250  ret.m_scalar = d * v.m_scalar;
251  return ret;
252 }
253 
254 fVec4 operator + (const fVec4& v1, const fVec4& v2)
255 {
256  fVec4 ret;
257  ret.m_vec = v1.m_vec + v2.m_vec;
258  ret.m_scalar = v1.m_scalar + v2.m_scalar;
259  return ret;
260 }
261 
262 fVec4 operator - (const fVec4& v1, const fVec4& v2)
263 {
264  fVec4 ret;
265  ret.m_vec = v1.m_vec - v2.m_vec;
266  ret.m_scalar = v1.m_scalar - v2.m_scalar;
267  return ret;
268 }
269 
271 {
272  m_vec.zero();
273  m_scalar = 0;
274 }
friend ostream & operator<<(ostream &ost, const fVec4 &mat)
Definition: fMatrix4.cpp:24
void add(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:888
double m_scalar
Definition: fMatrix4.h:86
fVec4 operator=(const fVec4 &vec)
Definition: fMatrix4.cpp:196
friend fVec3 & Vec(fVec4 &vec)
Definition: fMatrix4.h:115
void mul(fVec4 &_vec, double d)
Definition: fMatrix4.cpp:162
void zero()
Creates a zero vector.
Definition: fMatrix3.h:283
void operator*=(double)
Definition: fMatrix4.cpp:89
friend double & Scalar(fVec4 &vec)
Definition: fMatrix4.h:121
void cross(const fVec3 &vec1, const fVec3 &vec2)
Cross product.
Definition: fMatrix3.cpp:944
void add(const fVec4 &vec1, const fVec4 &vec2)
Definition: fMatrix4.cpp:156
void operator-=(const fVec4 &)
Definition: fMatrix4.cpp:215
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
void operator+=(const fVec4 &)
Definition: fMatrix4.cpp:209
double & operator()(int i, int j)
Definition: fMatrix4.cpp:30
void operator-=(const fMat44 &)
Definition: fMatrix4.cpp:82
png_uint_32 i
Definition: png.h:2735
fMat44 operator=(const fMat44 &mat)
Definition: fMatrix4.cpp:67
void operator/=(double)
Definition: fMatrix4.cpp:227
friend double operator*(const fVec4 &, const fVec4 &)
Definition: fMatrix4.cpp:241
void operator+=(const fMat44 &)
Definition: fMatrix4.cpp:75
void cross(const fVec4 &vec1, const fVec4 &vec2)
Definition: fMatrix4.cpp:16
fVec3 m_vec
Definition: fMatrix4.h:85
double m_scalar
Definition: fMatrix4.h:161
Definition: fMatrix4.h:90
void sub(const fVec4 &vec1, const fVec4 &vec2)
Definition: fMatrix4.cpp:150
void zero()
Definition: fMatrix4.cpp:270
friend fVec4 operator-(const fVec4 &)
Definition: fMatrix4.cpp:233
double & operator()(int i)
Definition: fMatrix4.cpp:168
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
double temp
Definition: fMatrix4.h:162
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916
fVec3 m_vec
Definition: fMatrix4.h:160
fMat33 m_mat
Definition: fMatrix4.h:84
void operator*=(double)
Definition: fMatrix4.cpp:221
friend fVec4 operator+(const fVec4 &, const fVec4 &)
Definition: fMatrix4.cpp:254
void operator/=(double)
Definition: fMatrix4.cpp:96


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:03