vpQuaternionVector.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * $Id: vpQuaternionVector.cpp 3514 2011-12-08 10:19:39Z fnovotny $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2011 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Quaternion vector.
36  *
37  * Authors:
38  * Filip Novotny
39  *
40  *****************************************************************************/
41 
42 #include <stdio.h>
43 #include <string.h>
44 #include <algorithm>
45 
46 #include <ros/ros.h>
47 #include <visp/vpConfig.h>
48 #include <visp/vpMath.h>
49 
50 #if VISP_VERSION_INT <= (2<<16 | 6<<8 | 1)
51 
53 
54 // minimum value of sine
55 const double vpQuaternionVector::minimum = 0.0001;
56 
62 
64 vpQuaternionVector::vpQuaternionVector(const double x, const double y,
65  const double z,const double w)
66  : vpColVector(4)
67 {
68  set(x, y, z, w);
69 }
70 
76 vpQuaternionVector::vpQuaternionVector(const vpRotationMatrix &R) :
77  vpColVector(4)
78 {
79  buildFrom(R);
80 }
86  vpColVector(4)
87 {
88  for(unsigned int i=0;i<size();i++) (*this)[i]=q.r[i];
89 }
90 
98 void vpQuaternionVector::set(const double x, const double y,
99  const double z,const double w)
100 {
101  r[0]=x;
102  r[1]=y;
103  r[2]=z;
104  r[3]=w;
105 }
106 
107 
116 {
117  return vpQuaternionVector(x()+q.x(), y()+q.y(), z()+q.z(), w()+q.w());
118 }
127 {
128  return vpQuaternionVector(x()-q.x(), y()-q.y(), z()-q.z(), w()-q.w());
129 }
130 
133 {
134  return vpQuaternionVector(-x(), -y(), -z(), -w());
135 }
136 
139 {
140  return vpQuaternionVector(l*x(),l*y(),l*z(),l*w());
141 }
142 
145  return vpQuaternionVector(w() * rq.x() + x() * rq.w() + y() * rq.z() - z() * rq.y(),
146  w() * rq.y() + y() * rq.w() + z() * rq.x() - x() * rq.z(),
147  w() * rq.z() + z() * rq.w() + x() * rq.y() - y() * rq.x(),
148  w() * rq.w() - x() * rq.x() - y() * rq.y() - z() * rq.z());
149 }
150 
153 {
154  for(unsigned int i=0;i<size();i++) (*this)[i]=q.r[i];
155  return *this;
156 }
157 
163 void vpQuaternionVector::buildFrom(const vpRotationMatrix &R)
164 {
165  double s,c,theta,sinc;
166  double axis_x,axis_y,axis_z;
167 
168  s = (R[1][0]-R[0][1])*(R[1][0]-R[0][1])
169  + (R[2][0]-R[0][2])*(R[2][0]-R[0][2])
170  + (R[2][1]-R[1][2])*(R[2][1]-R[1][2]);
171  s = sqrt(s)/2.0;
172  c = (R[0][0]+R[1][1]+R[2][2]-1.0)/2.0;
173  theta=atan2(s,c); /* theta in [0, PI] since s > 0 */
174 
175  if ((s > minimum) || (c > 0.0)) { /* general case */
176  sinc = vpMath::sinc(s,theta);
177 
178  axis_x = (R[2][1]-R[1][2])/(2*sinc);
179  axis_y = (R[0][2]-R[2][0])/(2*sinc);
180  axis_z = (R[1][0]-R[0][1])/(2*sinc);
181  } else { /* theta near PI */
182  axis_x = theta*(sqrt((R[0][0]-c)/(1-c)));
183  if ((R[2][1]-R[1][2]) < 0) axis_x = -axis_x;
184  axis_y = theta*(sqrt((R[1][1]-c)/(1-c)));
185  if ((R[0][2]-R[2][0]) < 0) axis_y = -axis_y;
186  axis_z = theta*(sqrt((R[2][2]-c)/(1-c)));
187  if ((R[1][0]-R[0][1]) < 0) axis_z = -axis_z;
188  }
189 
190  theta *= 0.5;
191  double norm = sqrt(axis_x*axis_x+axis_y*axis_y+axis_z*axis_z);
192  if(fabs(norm)<minimum) norm = 1.;
193  double sinTheta_2 = sin(theta);
194  set((axis_x * sinTheta_2)/norm,
195  (axis_y * sinTheta_2)/norm,
196  (axis_z * sinTheta_2)/norm,
197  cos(theta));
198 
199 }
200 
201 #endif
void set(const double x, const double y, const double z, const double w)
static const double minimum
vpQuaternionVector operator-()
Negate operator. Returns a quaternion defined by (-x,-y,-z-,-w).
XmlRpcServer s
vpQuaternionVector operator*(const double l)
Multiplication by scalar. Returns a quaternion defined by (lx,ly,lz,lw).
double y() const
returns y-component of the quaternion
Class that consider the case of a quaternion and basic operations on it.
double w() const
returns w-component of the quaternion
double z() const
returns z-component of the quaternion
double x() const
returns x-component of the quaternion
Defines a quaternion and its basic operations.
void buildFrom(const vpRotationMatrix &R)
vpQuaternionVector operator+(vpQuaternionVector &q)
vpQuaternionVector & operator=(vpQuaternionVector &q)
Copy operator. Allow operation such as Q = q.


visp_bridge
Author(s): Filip Novotny
autogenerated on Fri Nov 1 2019 03:27:20