-
Notifications
You must be signed in to change notification settings - Fork 0
/
vectormath.cpp
executable file
·96 lines (83 loc) · 2.49 KB
/
vectormath.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
#include "vectormath.h"
#include <Eigen/Dense>
#include <random>
using namespace Eigen;
const Matrix3d VectorMath::crossProductMatrix(const Eigen::Vector3d &v)
{
Matrix3d result;
result << 0, -v[2], v[1],
v[2], 0, -v[0],
-v[1], v[0], 0;
return result;
}
const Matrix3d VectorMath::rotationMatrix(const Vector3d &axisAngle)
{
double theta = axisAngle.norm();
Vector3d thetahat = axisAngle/theta;
if(theta == 0)
thetahat.setZero();
Matrix3d result;
result.setIdentity();
result = cos(theta)*result + sin(theta)*crossProductMatrix(thetahat) + (1-cos(theta))*thetahat*thetahat.transpose();
return result;
}
double VectorMath::randomUnitIntervalReal()
{
return double(rand())/double(RAND_MAX);
}
const Vector3d VectorMath::axisAngle(const Matrix3d &rotationMatrix)
{
Matrix3d RminusI;
RminusI.setIdentity();
RminusI = rotationMatrix - RminusI;
JacobiSVD<MatrixXd> svd(RminusI, ComputeFullV);
//assert(fabs(svd.singularValues()[2]) < 1e-8);
Vector3d axis = svd.matrixV().col(2);
Vector3d testAxis = perpToAxis(axis);
Vector3d resultAxis = rotationMatrix*testAxis;
double theta = atan2(testAxis.cross(resultAxis).dot(axis), testAxis.dot(resultAxis));
return theta*axis;
}
const Vector3d VectorMath::perpToAxis(const Vector3d &v)
{
int mincoord = 0;
double minval = 0;
for(int i=0; i<3; i++)
{
if(fabs(v[i]) < minval)
{
mincoord = i;
minval = fabs(v[i]);
}
}
Vector3d other(0,0,0);
other[mincoord] = 1.0;
Vector3d result = v.cross(other);
result.normalize();
return result;
}
const Matrix3d VectorMath::TMatrix(const Vector3d &v)
{
double vnormsq = v.dot(v);
Matrix3d I;
I.setIdentity();
if(vnormsq == 0)
return I;
Matrix3d R = rotationMatrix(v);
return (v*v.transpose() + (R.transpose()-I)*crossProductMatrix(v))/vnormsq;
}
const Matrix3d VectorMath::DrotVector(const Vector3d &axisangle, const Vector3d &rotatingVector)
{
Matrix3d R = rotationMatrix(axisangle);
Matrix3d result = -R * crossProductMatrix(rotatingVector) * TMatrix(axisangle);
return result;
}
// const Eigen::Vector3d VectorMath::randomPointOnSphere()
// {
// std::random_device r;
// std::mt19937 generator(r());
// std::normal_distribution<double> distribution(0.0,1.0);
// Vector3d vec(distribution(generator), distribution(generator), distribution(generator));
// vec /= vec.norm();
// return vec;
// }