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 <string>
12 
13 #include <yarp/os/Log.h>
14 #include <yarp/math/SVD.h>
15 #include <iCub/ctrl/math.h>
16 
17 using namespace std;
18 using namespace yarp::os;
19 using namespace yarp::sig;
20 using namespace yarp::math;
21 
22 
23 /************************************************************************/
24 double iCub::ctrl::dot(const Matrix &A, int colA, const Matrix &B, int colB)
25 {
26  double ret=0.0;
27  size_t nrowsA=A.rows();
28  size_t nrowsB=B.rows();
29  size_t nrows=nrowsA<nrowsB ? nrowsA : nrowsB;
30 
31  for (unsigned int i=0; i<nrows; i++)
32  ret+=A(i,colA)*B(i,colB);
33 
34  return ret;
35 }
36 
37 
38 /************************************************************************/
39 double iCub::ctrl::norm(const Matrix &M, int col)
40 {
41  return sqrt(norm2(M,col));
42 }
43 
44 
45 /************************************************************************/
46 Vector iCub::ctrl::cross(const Matrix &A, int colA, const Matrix &B, int colB)
47 {
48  yAssert((A.rows()>=3) && (B.rows()>=3));
49 
50  Vector v(3);
51  v[0]=A(1,colA)*B(2,colB)-A(2,colA)*B(1,colB);
52  v[1]=A(2,colA)*B(0,colB)-A(0,colA)*B(2,colB);
53  v[2]=A(0,colA)*B(1,colB)-A(1,colA)*B(0,colB);
54 
55  return v;
56 }
57 
58 
59 /************************************************************************/
60 Vector iCub::ctrl::Dcross(const Vector &a, const Vector &Da, const Vector &b,
61  const Vector &Db)
62 {
63  yAssert((a.length()>=3) && (b.length()>=3) &&
64  (Da.length()>=3) && (Db.length()>=3));
65 
66  Vector Dv(3);
67  Dv[0]=Da[1]*b[2]+a[1]*Db[2]-Da[2]*b[1]-a[2]*Db[1];
68  Dv[1]=Da[2]*b[0]+a[2]*Db[0]-Da[0]*b[2]-a[0]*Db[2];
69  Dv[2]=Da[0]*b[1]+a[0]*Db[1]-Da[1]*b[0]-a[1]*Db[0];
70 
71  return Dv;
72 }
73 
74 
75 /************************************************************************/
76 Vector iCub::ctrl::Dcross(const Matrix &A, const Matrix &DA, int colA,
77  const Matrix &B, const Matrix &DB, int colB)
78 {
79  yAssert((A.rows()>=3) && (B.rows()>=3) &&
80  (DA.rows()>=3) && (DB.rows()>=3));
81 
82  Vector Dv(3);
83  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);
84  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);
85  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);
86 
87  return Dv;
88 }
89 
90 
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).
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:91
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
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.
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).
A
Definition: sine.m:16