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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
|
#include "types.hpp"
#include <osg/io_utils>
namespace Stereo
{
Pose Pose::operator+(const Pose& rhs)
{
Pose pose = *this;
pose.position += this->orientation * rhs.position;
pose.orientation = rhs.orientation * this->orientation;
return pose;
}
const Pose& Pose::operator+=(const Pose& rhs)
{
*this = *this + rhs;
return *this;
}
Pose Pose::operator*(float scalar)
{
Pose pose = *this;
pose.position *= scalar;
return pose;
}
const Pose& Pose::operator*=(float scalar)
{
*this = *this * scalar;
return *this;
}
Pose Pose::operator/(float scalar)
{
Pose pose = *this;
pose.position /= scalar;
return pose;
}
const Pose& Pose::operator/=(float scalar)
{
*this = *this / scalar;
return *this;
}
bool Pose::operator==(const Pose& rhs) const
{
return position == rhs.position && orientation == rhs.orientation;
}
osg::Matrix View::viewMatrix(bool useGLConventions)
{
auto position = pose.position;
auto orientation = pose.orientation;
if (useGLConventions)
{
// When applied as an offset to an existing view matrix,
// that view matrix will already convert points to a camera space
// with opengl conventions. So we need to convert offsets to opengl
// conventions.
float y = position.y();
float z = position.z();
position.y() = z;
position.z() = -y;
y = orientation.y();
z = orientation.z();
orientation.y() = z;
orientation.z() = -y;
osg::Matrix viewMatrix;
viewMatrix.setTrans(-position);
viewMatrix.postMultRotate(orientation.conj());
return viewMatrix;
}
else
{
osg::Vec3d forward = orientation * osg::Vec3d(0, 1, 0);
osg::Vec3d up = orientation * osg::Vec3d(0, 0, 1);
osg::Matrix viewMatrix;
viewMatrix.makeLookAt(position, position + forward, up);
return viewMatrix;
}
}
osg::Matrix View::perspectiveMatrix(float near, float far, bool reverseZ)
{
const float tanLeft = tanf(fov.angleLeft);
const float tanRight = tanf(fov.angleRight);
const float tanDown = tanf(fov.angleDown);
const float tanUp = tanf(fov.angleUp);
const float tanWidth = tanRight - tanLeft;
const float tanHeight = tanUp - tanDown;
float matrix[16] = {};
matrix[0] = 2 / tanWidth;
matrix[4] = 0;
matrix[8] = (tanRight + tanLeft) / tanWidth;
matrix[12] = 0;
matrix[1] = 0;
matrix[5] = 2 / tanHeight;
matrix[9] = (tanUp + tanDown) / tanHeight;
matrix[13] = 0;
if (reverseZ)
{
matrix[2] = 0;
matrix[6] = 0;
matrix[10] = (2.f * near) / (far - near);
matrix[14] = ((2.f * near) * far) / (far - near);
}
else
{
matrix[2] = 0;
matrix[6] = 0;
matrix[10] = -(far + near) / (far - near);
matrix[14] = -(far * (2.f * near)) / (far - near);
}
matrix[3] = 0;
matrix[7] = 0;
matrix[11] = -1;
matrix[15] = 0;
return osg::Matrix(matrix);
}
bool FieldOfView::operator==(const FieldOfView& rhs) const
{
return angleDown == rhs.angleDown && angleUp == rhs.angleUp && angleLeft == rhs.angleLeft
&& angleRight == rhs.angleRight;
}
bool View::operator==(const View& rhs) const
{
return pose == rhs.pose && fov == rhs.fov;
}
std::ostream& operator<<(std::ostream& os, const Pose& pose)
{
os << "position=" << pose.position << ", orientation=" << pose.orientation;
return os;
}
std::ostream& operator<<(std::ostream& os, const FieldOfView& fov)
{
os << "left=" << fov.angleLeft << ", right=" << fov.angleRight << ", down=" << fov.angleDown
<< ", up=" << fov.angleUp;
return os;
}
std::ostream& operator<<(std::ostream& os, const View& view)
{
os << "pose=< " << view.pose << " >, fov=< " << view.fov << " >";
return os;
}
}
|
