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
|
// Copyright 2006-2007 Nanorex, Inc. See LICENSE file for details.
/*
Name: rotationmatrix.cpp
Author: Oleksandr Shevchenko
Description: class for rotation in 3-D space
*/
#include "rotationmatrix.h"
//----------------------------------------------------------------------------
// Constructor
RotationMatrix::RotationMatrix()
{
mPx = Triple(1,0,0);
mPy = Triple(0,1,0);
mPz = Triple(0,0,1);
}
//----------------------------------------------------------------------------
// Constructor
RotationMatrix::RotationMatrix(
const Triple & p0,
const Triple & p1,
const Triple & p2):
mPx( // first row of rotation matrix
p0),
mPy( // second row of rotation matrix
p1),
mPz( // third row of rotation matrix
p2)
{
}
//----------------------------------------------------------------------------
// Calculate()
//
// apply rotation
//
Triple RotationMatrix::Calculate(
const Triple & p)
{
double x = p % mPx;
double y = p % mPy;
double z = p % mPz;
return (Triple(x,y,z));
}
//----------------------------------------------------------------------------
// Initialize()
//
// formation of rotation matrix
//
void RotationMatrix::Initialize(
const Triple & v,
double ca)
{
double eps = 1e-10;
// v - rotation axis
// ca - cosinus of rotation angle
double vlen = v.Len();
if (vlen < eps )
{
// if no rotation
if (ca < eps - 1)
{
// angle == pi
mPx = Triple(-1,0,0);
mPy = Triple(0,-1,0);
mPz = Triple(0,0,-1);
}
return;
}
// calculate constants
double sa = sqrt(1 - ca*ca);
double c1 = 1 - ca;
double vx = v.X()/vlen, vy = v.Y()/vlen, vz = v.Z()/vlen;
double vxy = vx*vy, vxz = vx*vz, vyz = vy*vz;
// formation of rotation matrix
mPx = Triple(vx * vx * c1 + ca, vxy * c1 + sa * vz, vxz * c1 - sa * vy);
mPy = Triple(vxy * c1 - sa * vz, vy * vy * c1 + ca, vyz * c1 + sa * vx);
mPz = Triple(vxz * c1 + sa * vy, vyz * c1 - sa * vx, vz * vz * c1 + ca);
}
|
