iCub-main
math.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms
7  * of the BSD-3-Clause license. See the accompanying LICENSE file for
8  * details.
9 */
10 
11 #include <cmath>
12 #include <string>
13 
14 #include <yarp/os/Log.h>
15 #include <yarp/math/SVD.h>
16 #include <iCub/ctrl/math.h>
17 
18 using namespace std;
19 using namespace yarp::os;
20 using namespace yarp::sig;
21 using namespace yarp::math;
22 
23 
24 /************************************************************************/
25 namespace iCub {
26  namespace ctrl {
27  const double CTRL_PI=M_PI;
28  const double CTRL_RAD2DEG=180.0/M_PI;
29  const double CTRL_DEG2RAD=M_PI/180.0;
30  }
31 }
32 
33 
34 /************************************************************************/
35 double iCub::ctrl::dot(const Matrix &A, int colA, const Matrix &B, int colB)
36 {
37  double ret=0.0;
38  size_t nrowsA=A.rows();
39  size_t nrowsB=B.rows();
40  size_t nrows=nrowsA<nrowsB ? nrowsA : nrowsB;
41 
42  for (unsigned int i=0; i<nrows; i++)
43  ret+=A(i,colA)*B(i,colB);
44 
45  return ret;
46 }
47 
48 
49 /************************************************************************/
50 double iCub::ctrl::norm(const Matrix &M, int col)
51 {
52  return sqrt(norm2(M,col));
53 }
54 
55 
56 /************************************************************************/
57 Vector iCub::ctrl::cross(const Matrix &A, int colA, const Matrix &B, int colB)
58 {
59  yAssert((A.rows()>=3) && (B.rows()>=3));
60 
61  Vector v(3);
62  v[0]=A(1,colA)*B(2,colB)-A(2,colA)*B(1,colB);
63  v[1]=A(2,colA)*B(0,colB)-A(0,colA)*B(2,colB);
64  v[2]=A(0,colA)*B(1,colB)-A(1,colA)*B(0,colB);
65 
66  return v;
67 }
68 
69 
70 /************************************************************************/
71 Vector iCub::ctrl::Dcross(const Vector &a, const Vector &Da, const Vector &b,
72  const Vector &Db)
73 {
74  yAssert((a.length()>=3) && (b.length()>=3) &&
75  (Da.length()>=3) && (Db.length()>=3));
76 
77  Vector Dv(3);
78  Dv[0]=Da[1]*b[2]+a[1]*Db[2]-Da[2]*b[1]-a[2]*Db[1];
79  Dv[1]=Da[2]*b[0]+a[2]*Db[0]-Da[0]*b[2]-a[0]*Db[2];
80  Dv[2]=Da[0]*b[1]+a[0]*Db[1]-Da[1]*b[0]-a[1]*Db[0];
81 
82  return Dv;
83 }
84 
85 
86 /************************************************************************/
87 Vector iCub::ctrl::Dcross(const Matrix &A, const Matrix &DA, int colA,
88  const Matrix &B, const Matrix &DB, int colB)
89 {
90  yAssert((A.rows()>=3) && (B.rows()>=3) &&
91  (DA.rows()>=3) && (DB.rows()>=3));
92 
93  Vector Dv(3);
94  Dv[0]=DA(1,colA)*B(2,colB)+A(1,colA)*DB(2,colB)-DA(2,colA)*B(1,colB)-A(2,colA)*DB(1,colB);
95  Dv[1]=DA(2,colA)*B(0,colB)+A(2,colA)*DB(0,colB)-DA(0,colA)*B(2,colB)-A(0,colA)*DB(2,colB);
96  Dv[2]=DA(0,colA)*B(1,colB)+A(0,colA)*DB(1,colB)-DA(1,colA)*B(0,colB)-A(1,colA)*DB(0,colB);
97 
98  return Dv;
99 }
100 
101 
#define M_PI
Definition: XSensMTx.cpp:24
const double CTRL_PI
The PI constant.
Definition: math.cpp:27
STL namespace.
double dot(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
Returns the dot product between two vectors given in the form: matrix(:,col).
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
double norm2(const yarp::sig::Matrix &M, int col)
Returns the squared norm of the vector given in the form: matrix(:,col).
Definition: math.h:89
static int v
Definition: iCub_Sim.cpp:42
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
A
Definition: sine.m:16
yarp::sig::Vector cross(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
Returns the cross product between two vectors given in the form: matrix(:,col).
This file contains the definition of unique IDs for the body parts and the skin parts of the robot...
#define CTRL_RAD2DEG
Definition: XSensMTx.cpp:25
yarp::sig::Vector Dcross(const yarp::sig::Vector &a, const yarp::sig::Vector &Da, const yarp::sig::Vector &b, const yarp::sig::Vector &Db)
Returns the derivatice of cross product between two vectors.