Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
7e98c607
Commit
7e98c607
authored
Dec 03, 2010
by
hengli
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Fixed outstanding imagery issues except for one: OSG is unexpectedly culling tiles.
parent
64a56635
Changes
10
Hide whitespace changes
Inline
Side-by-side
Showing
10 changed files
with
165 additions
and
145 deletions
+165
-145
Freenect.cc
src/input/Freenect.cc
+4
-4
GCManipulator.cc
src/ui/map3D/GCManipulator.cc
+24
-24
GCManipulator.h
src/ui/map3D/GCManipulator.h
+6
-6
Imagery.cc
src/ui/map3D/Imagery.cc
+39
-19
Pixhawk3DWidget.cc
src/ui/map3D/Pixhawk3DWidget.cc
+55
-37
Pixhawk3DWidget.h
src/ui/map3D/Pixhawk3DWidget.h
+7
-6
Q3DWidget.cc
src/ui/map3D/Q3DWidget.cc
+2
-14
Q3DWidget.h
src/ui/map3D/Q3DWidget.h
+2
-16
Texture.cc
src/ui/map3D/Texture.cc
+23
-16
Texture.h
src/ui/map3D/Texture.h
+3
-3
No files found.
src/input/Freenect.cc
View file @
7e98c607
...
...
@@ -197,7 +197,7 @@ Freenect::get3DPointCloudData(void)
unsigned
short
*
data
=
reinterpret_cast
<
unsigned
short
*>
(
depth
);
for
(
int
i
=
0
;
i
<
FREENECT_FRAME_PIX
;
++
i
)
{
if
(
data
[
i
]
<=
2048
)
if
(
data
[
i
]
>
0
&&
data
[
i
]
<=
2048
)
{
// see www.ros.org/wiki/kinect_node for details
double
range
=
1.0
f
/
(
-
0.00307
f
*
static_cast
<
float
>
(
data
[
i
])
+
3.33
f
);
...
...
@@ -226,9 +226,9 @@ Freenect::get6DPointCloudData(void)
{
Vector6D
point
;
point
.
x
=
rawPointCloud
[
i
]
.
x
();
point
.
y
=
rawPointCloud
[
i
]
.
y
();
point
.
z
=
rawPointCloud
[
i
]
.
z
();
point
.
x
=
rawPointCloud
.
at
(
i
)
.
x
();
point
.
y
=
rawPointCloud
.
at
(
i
)
.
y
();
point
.
z
=
rawPointCloud
.
at
(
i
)
.
z
();
QVector4D
transformedPoint
=
transformMatrix
*
QVector4D
(
point
.
x
,
point
.
y
,
point
.
z
,
1.0
);
...
...
src/ui/map3D/GCManipulator.cc
View file @
7e98c607
...
...
@@ -33,21 +33,21 @@ This file is part of the QGROUNDCONTROL project
GCManipulator
::
GCManipulator
()
{
_moveSensitivity
=
0.05
f
;
_zoomSensitivity
=
1.0
f
;
_minZoomRange
=
2.0
f
;
_moveSensitivity
=
0.05
;
_zoomSensitivity
=
1.0
;
_minZoomRange
=
2.0
;
}
void
GCManipulator
::
setMinZoomRange
(
float
minZoomRange
)
GCManipulator
::
setMinZoomRange
(
double
minZoomRange
)
{
_minZoomRange
=
minZoomRange
;
}
void
GCManipulator
::
move
(
float
dx
,
float
dy
,
float
dz
)
GCManipulator
::
move
(
double
dx
,
double
dy
,
double
dz
)
{
_center
+=
osg
::
Vec3
(
dx
,
dy
,
dz
);
_center
+=
osg
::
Vec3
d
(
dx
,
dy
,
dz
);
}
bool
...
...
@@ -126,15 +126,15 @@ GCManipulator::handle(const osgGA::GUIEventAdapter& ea,
case
GUIEventAdapter
:
:
SCROLL
:
{
// zoom model
float
scale
=
1.0
f
;
double
scale
=
1.0
;
if
(
ea
.
getScrollingMotion
()
==
GUIEventAdapter
::
SCROLL_UP
)
{
scale
-=
_zoomSensitivity
*
0.1
f
;
scale
-=
_zoomSensitivity
*
0.1
;
}
else
{
scale
+=
_zoomSensitivity
*
0.1
f
;
scale
+=
_zoomSensitivity
*
0.1
;
}
if
(
_distance
*
scale
>
_minZoomRange
)
{
...
...
@@ -161,12 +161,12 @@ GCManipulator::handle(const osgGA::GUIEventAdapter& ea,
}
case
GUIEventAdapter
:
:
KEY_Left
:
{
float
scale
=
-
_moveSensitivity
*
_distance
;
double
scale
=
-
_moveSensitivity
*
_distance
;
osg
::
Matrix
rotation_matrix
;
rotation_matrix
.
makeRotate
(
_rotation
);
osg
::
Vec3
dv
(
scale
,
0.0
f
,
0.0
f
);
osg
::
Vec3
d
dv
(
scale
,
0.0
,
0.0
);
_center
+=
dv
*
rotation_matrix
;
...
...
@@ -174,12 +174,12 @@ GCManipulator::handle(const osgGA::GUIEventAdapter& ea,
}
case
GUIEventAdapter
:
:
KEY_Right
:
{
float
scale
=
_moveSensitivity
*
_distance
;
double
scale
=
_moveSensitivity
*
_distance
;
osg
::
Matrix
rotation_matrix
;
rotation_matrix
.
makeRotate
(
_rotation
);
osg
::
Vec3
dv
(
scale
,
0.0
f
,
0.0
f
);
osg
::
Vec3
d
dv
(
scale
,
0.0
,
0.0
);
_center
+=
dv
*
rotation_matrix
;
...
...
@@ -187,12 +187,12 @@ GCManipulator::handle(const osgGA::GUIEventAdapter& ea,
}
case
GUIEventAdapter
:
:
KEY_Up
:
{
float
scale
=
_moveSensitivity
*
_distance
;
double
scale
=
_moveSensitivity
*
_distance
;
osg
::
Matrix
rotation_matrix
;
rotation_matrix
.
makeRotate
(
_rotation
);
osg
::
Vec3
dv
(
0.0
f
,
scale
,
0.0
f
);
osg
::
Vec3
d
dv
(
0.0
,
scale
,
0.0
);
_center
+=
dv
*
rotation_matrix
;
...
...
@@ -200,12 +200,12 @@ GCManipulator::handle(const osgGA::GUIEventAdapter& ea,
}
case
GUIEventAdapter
:
:
KEY_Down
:
{
float
scale
=
-
_moveSensitivity
*
_distance
;
double
scale
=
-
_moveSensitivity
*
_distance
;
osg
::
Matrix
rotation_matrix
;
rotation_matrix
.
makeRotate
(
_rotation
);
osg
::
Vec3
dv
(
0.0
f
,
scale
,
0.0
f
);
osg
::
Vec3
d
dv
(
0.0
,
scale
,
0.0
);
_center
+=
dv
*
rotation_matrix
;
...
...
@@ -231,7 +231,7 @@ GCManipulator::handle(const osgGA::GUIEventAdapter& ea,
bool
GCManipulator
::
calcMovement
()
GCManipulator
::
calcMovement
(
void
)
{
using
namespace
osgGA
;
...
...
@@ -241,11 +241,11 @@ GCManipulator::calcMovement()
return
false
;
}
float
dx
=
_ga_t0
->
getXnormalized
()
-
_ga_t1
->
getXnormalized
();
float
dy
=
_ga_t0
->
getYnormalized
()
-
_ga_t1
->
getYnormalized
();
double
dx
=
_ga_t0
->
getXnormalized
()
-
_ga_t1
->
getXnormalized
();
double
dy
=
_ga_t0
->
getYnormalized
()
-
_ga_t1
->
getYnormalized
();
// return if there is no movement.
if
(
dx
==
0.0
f
&&
dy
==
0.0
f
)
if
(
dx
==
0.0
&&
dy
==
0.0
)
{
return
false
;
}
...
...
@@ -282,12 +282,12 @@ GCManipulator::calcMovement()
GUIEventAdapter
::
RIGHT_MOUSE_BUTTON
))
{
// pan model
float
scale
=
-
_moveSensitivity
*
_distance
;
double
scale
=
-
_moveSensitivity
*
_distance
;
osg
::
Matrix
rotation_matrix
;
rotation_matrix
.
makeRotate
(
_rotation
);
osg
::
Vec3
dv
(
dx
*
scale
,
dy
*
scale
,
0.0
f
);
osg
::
Vec3
d
dv
(
dx
*
scale
,
dy
*
scale
,
0.0
);
_center
+=
dv
*
rotation_matrix
;
...
...
@@ -297,7 +297,7 @@ GCManipulator::calcMovement()
else
if
(
buttonMask
==
GUIEventAdapter
::
RIGHT_MOUSE_BUTTON
)
{
// zoom model
float
scale
=
1.0
f
+
dy
*
_zoomSensitivity
;
double
scale
=
1.0
+
dy
*
_zoomSensitivity
;
if
(
_distance
*
scale
>
_minZoomRange
)
{
...
...
src/ui/map3D/GCManipulator.h
View file @
7e98c607
...
...
@@ -39,9 +39,9 @@ class GCManipulator : public osgGA::TrackballManipulator
public:
GCManipulator
();
void
setMinZoomRange
(
float
minZoomRange
);
void
setMinZoomRange
(
double
minZoomRange
);
virtual
void
move
(
float
dx
,
float
dy
,
float
dz
);
virtual
void
move
(
double
dx
,
double
dy
,
double
dz
);
/**
* @brief Handle events.
...
...
@@ -51,11 +51,11 @@ public:
osgGA
::
GUIActionAdapter
&
us
);
protected:
bool
calcMovement
();
bool
calcMovement
(
void
);
float
_moveSensitivity
;
float
_zoomSensitivity
;
float
_minZoomRange
;
double
_moveSensitivity
;
double
_zoomSensitivity
;
double
_minZoomRange
;
};
#endif // GCMANIPULATOR_H
src/ui/map3D/Imagery.cc
View file @
7e98c607
...
...
@@ -70,6 +70,11 @@ Imagery::prefetch2D(double windowWidth, double windowHeight,
double
zoom
,
double
xOrigin
,
double
yOrigin
,
const
QString
&
utmZone
)
{
if
(
currentImageryType
==
BLANK_MAP
)
{
return
;
}
double
tileResolution
;
if
(
currentImageryType
==
GOOGLE_SATELLITE
||
currentImageryType
==
GOOGLE_MAP
)
...
...
@@ -115,6 +120,16 @@ Imagery::draw2D(double windowWidth, double windowHeight,
double
zoom
,
double
xOrigin
,
double
yOrigin
,
const
QString
&
utmZone
)
{
if
(
getNumDrawables
()
>
0
)
{
removeDrawables
(
0
,
getNumDrawables
());
}
if
(
currentImageryType
==
BLANK_MAP
)
{
return
;
}
double
tileResolution
;
if
(
currentImageryType
==
GOOGLE_SATELLITE
||
currentImageryType
==
GOOGLE_MAP
)
...
...
@@ -144,11 +159,6 @@ Imagery::draw2D(double windowWidth, double windowHeight,
yOrigin
+
windowHeight
/
2.0
/
zoom
*
1.5
,
utmZone
,
minTileX
,
minTileY
,
maxTileX
,
maxTileY
,
zoomLevel
);
if
(
getNumDrawables
()
>
0
)
{
removeDrawables
(
0
,
getNumDrawables
());
}
for
(
int
r
=
minTileY
;
r
<=
maxTileY
;
++
r
)
{
for
(
int
c
=
minTileX
;
c
<=
maxTileX
;
++
c
)
...
...
@@ -161,10 +171,10 @@ Imagery::draw2D(double windowWidth, double windowHeight,
TexturePtr
t
=
textureCache
->
get
(
tileURL
);
if
(
!
t
.
isNull
())
{
addDrawable
(
t
->
draw
(
y1
-
yOrigin
,
x1
-
xOrigin
,
y2
-
yOrigin
,
x2
-
xOrigin
,
y3
-
yOrigin
,
x3
-
xOrigin
,
y4
-
yOrigin
,
x4
-
xOrigin
,
addDrawable
(
t
->
draw
(
y1
,
x1
,
y2
,
x2
,
y3
,
x3
,
y4
,
x4
,
true
));
}
}
...
...
@@ -176,6 +186,11 @@ Imagery::prefetch3D(double radius, double tileResolution,
double
xOrigin
,
double
yOrigin
,
const
QString
&
utmZone
)
{
if
(
currentImageryType
==
BLANK_MAP
)
{
return
;
}
int
minTileX
,
minTileY
,
maxTileX
,
maxTileY
;
int
zoomLevel
;
...
...
@@ -200,6 +215,16 @@ Imagery::draw3D(double radius, double tileResolution,
double
xOrigin
,
double
yOrigin
,
const
QString
&
utmZone
)
{
if
(
getNumDrawables
()
>
0
)
{
removeDrawables
(
0
,
getNumDrawables
());
}
if
(
currentImageryType
==
BLANK_MAP
)
{
return
;
}
int
minTileX
,
minTileY
,
maxTileX
,
maxTileY
;
int
zoomLevel
;
...
...
@@ -208,11 +233,6 @@ Imagery::draw3D(double radius, double tileResolution,
xOrigin
+
radius
,
yOrigin
+
radius
,
utmZone
,
minTileX
,
minTileY
,
maxTileX
,
maxTileY
,
zoomLevel
);
if
(
getNumDrawables
()
>
0
)
{
removeDrawables
(
0
,
getNumDrawables
());
}
for
(
int
r
=
minTileY
;
r
<=
maxTileY
;
++
r
)
{
for
(
int
c
=
minTileX
;
c
<=
maxTileX
;
++
c
)
...
...
@@ -226,10 +246,10 @@ Imagery::draw3D(double radius, double tileResolution,
if
(
!
t
.
isNull
())
{
addDrawable
(
t
->
draw
(
y1
-
yOrigin
,
x1
-
xOrigin
,
y2
-
yOrigin
,
x2
-
xOrigin
,
y3
-
yOrigin
,
x3
-
xOrigin
,
y4
-
yOrigin
,
x4
-
xOrigin
,
addDrawable
(
t
->
draw
(
y1
,
x1
,
y2
,
x2
,
y3
,
x3
,
y4
,
x4
,
true
));
}
}
...
...
@@ -563,7 +583,7 @@ Imagery::UTMtoLL(double utmNorthing, double utmEasting, const QString& utmZone,
*
D
*
D
*
D
*
D
*
D
/
120.0
)
/
cos
(
phi1Rad
);
longitude
=
LongOrigin
+
longitude
/
M_PI
*
180.0
;
}
#include <QDebug>
QString
Imagery
::
getTileLocation
(
int
tileX
,
int
tileY
,
int
zoomLevel
,
double
tileResolution
)
const
...
...
src/ui/map3D/Pixhawk3DWidget.cc
View file @
7e98c607
...
...
@@ -56,6 +56,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
,
displayRGBD3D
(
false
)
,
enableRGBDColor
(
true
)
,
followCamera
(
true
)
,
enableFreenect
(
false
)
,
lastRobotX
(
0.0
f
)
,
lastRobotY
(
0.0
f
)
,
lastRobotZ
(
0.0
f
)
...
...
@@ -88,12 +89,15 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
#ifdef QGC_LIBFREENECT_ENABLED
freenect
.
reset
(
new
Freenect
());
freenect
->
init
();
enableFreenect
=
freenect
->
init
();
#endif
// generate RGBD model
rgbd3DNode
=
createRGBD3D
();
egocentricMap
->
addChild
(
rgbd3DNode
);
if
(
enableFreenect
)
{
rgbd3DNode
=
createRGBD3D
();
egocentricMap
->
addChild
(
rgbd3DNode
);
}
setupHUD
();
...
...
@@ -174,6 +178,15 @@ void
Pixhawk3DWidget
::
selectMapSource
(
int
index
)
{
mapNode
->
setImageryType
(
static_cast
<
Imagery
::
ImageryType
>
(
index
));
if
(
mapNode
->
getImageryType
()
==
Imagery
::
BLANK_MAP
)
{
displayImagery
=
false
;
}
else
{
displayImagery
=
true
;
}
}
void
...
...
@@ -187,7 +200,7 @@ Pixhawk3DWidget::selectVehicleModel(int index)
void
Pixhawk3DWidget
::
recenter
(
void
)
{
float
robotX
=
0.0
f
,
robotY
=
0.0
f
,
robotZ
=
0.0
f
;
double
robotX
=
0.0
f
,
robotY
=
0.0
f
,
robotZ
=
0.0
f
;
if
(
uas
!=
NULL
)
{
robotX
=
uas
->
getLocalX
();
...
...
@@ -345,12 +358,12 @@ Pixhawk3DWidget::display(void)
return
;
}
float
robotX
=
uas
->
getLocalX
();
float
robotY
=
uas
->
getLocalY
();
float
robotZ
=
uas
->
getLocalZ
();
float
robotRoll
=
uas
->
getRoll
();
float
robotPitch
=
uas
->
getPitch
();
float
robotYaw
=
uas
->
getYaw
();
double
robotX
=
uas
->
getLocalX
();
double
robotY
=
uas
->
getLocalY
();
double
robotZ
=
uas
->
getLocalZ
();
double
robotRoll
=
uas
->
getRoll
();
double
robotPitch
=
uas
->
getPitch
();
double
robotYaw
=
uas
->
getYaw
();
if
(
lastRobotX
==
0.0
f
&&
lastRobotY
==
0.0
f
&&
lastRobotZ
==
0.0
f
)
{
...
...
@@ -365,17 +378,17 @@ Pixhawk3DWidget::display(void)
if
(
followCamera
)
{
float
dx
=
robotY
-
lastRobotY
;
float
dy
=
robotX
-
lastRobotX
;
float
dz
=
lastRobotZ
-
robotZ
;
double
dx
=
robotY
-
lastRobotY
;
double
dy
=
robotX
-
lastRobotX
;
double
dz
=
lastRobotZ
-
robotZ
;
moveCamera
(
dx
,
dy
,
dz
);
}
robotPosition
->
setPosition
(
osg
::
Vec3
(
robotY
,
robotX
,
-
robotZ
));
robotAttitude
->
setAttitude
(
osg
::
Quat
(
-
robotYaw
,
osg
::
Vec3
f
(
0.0
f
,
0.0
f
,
1.0
f
),
robotPitch
,
osg
::
Vec3
f
(
1.0
f
,
0.0
f
,
0.0
f
),
robotRoll
,
osg
::
Vec3
f
(
0.0
f
,
1.0
f
,
0.0
f
)));
robotPosition
->
setPosition
(
osg
::
Vec3
d
(
robotY
,
robotX
,
-
robotZ
));
robotAttitude
->
setAttitude
(
osg
::
Quat
(
-
robotYaw
,
osg
::
Vec3
d
(
0.0
f
,
0.0
f
,
1.0
f
),
robotPitch
,
osg
::
Vec3
d
(
1.0
f
,
0.0
f
,
0.0
f
),
robotRoll
,
osg
::
Vec3
d
(
0.0
f
,
1.0
f
,
0.0
f
)));
if
(
displayTrail
)
{
...
...
@@ -398,7 +411,7 @@ Pixhawk3DWidget::display(void)
}
#ifdef QGC_LIBFREENECT_ENABLED
if
(
displayRGBD2D
||
displayRGBD3D
)
if
(
enableFreenect
&&
(
displayRGBD2D
||
displayRGBD3D
)
)
{
updateRGBD
();
}
...
...
@@ -409,10 +422,13 @@ Pixhawk3DWidget::display(void)
rollingMap
->
setChildValue
(
gridNode
,
displayGrid
);
rollingMap
->
setChildValue
(
trailNode
,
displayTrail
);
allocentricMap
->
setChildValue
(
mapNode
,
displayImagery
);
rollingMap
->
setChildValue
(
targetNode
,
displayTarget
);
rollingMap
->
setChildValue
(
waypointsNode
,
displayWaypoints
);
egocentricMap
->
setChildValue
(
rgbd3DNode
,
displayRGBD3D
);
allocentricMap
->
setChildValue
(
mapNode
,
displayImagery
);
if
(
enableFreenect
)
{
egocentricMap
->
setChildValue
(
rgbd3DNode
,
displayRGBD3D
);
}
hudGroup
->
setChildValue
(
rgb2DGeode
,
displayRGBD2D
);
hudGroup
->
setChildValue
(
depth2DGeode
,
displayRGBD2D
);
...
...
@@ -437,9 +453,6 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
case
'c'
:
case
'C'
:
enableRGBDColor
=
!
enableRGBDColor
;
break
;
case
'i'
:
case
'I'
:
displayImagery
=
!
displayImagery
;
break
;
}
}
...
...
@@ -531,7 +544,7 @@ Pixhawk3DWidget::createTrail(void)
trailGeometry
->
setUseDisplayList
(
false
);
geode
->
addDrawable
(
trailGeometry
.
get
());
trailVertices
=
new
osg
::
Vec3Array
;
trailVertices
=
new
osg
::
Vec3
d
Array
;
trailGeometry
->
setVertexArray
(
trailVertices
);
trailDrawArrays
=
new
osg
::
DrawArrays
(
osg
::
PrimitiveSet
::
LINE_STRIP
);
...
...
@@ -677,8 +690,8 @@ Pixhawk3DWidget::resizeHUD(void)
}
void
Pixhawk3DWidget
::
updateHUD
(
float
robotX
,
float
robotY
,
float
robotZ
,
float
robotRoll
,
float
robotPitch
,
float
robotYaw
)
Pixhawk3DWidget
::
updateHUD
(
double
robotX
,
double
robotY
,
double
robotZ
,
double
robotRoll
,
double
robotPitch
,
double
robotYaw
)
{
resizeHUD
();
...
...
@@ -715,7 +728,7 @@ Pixhawk3DWidget::updateHUD(float robotX, float robotY, float robotZ,
}
void
Pixhawk3DWidget
::
updateTrail
(
float
robotX
,
float
robotY
,
float
robotZ
)
Pixhawk3DWidget
::
updateTrail
(
double
robotX
,
double
robotY
,
double
robotZ
)
{
if
(
robotX
==
0.0
f
||
robotY
==
0.0
f
||
robotZ
==
0.0
f
)
{
...
...
@@ -725,9 +738,9 @@ Pixhawk3DWidget::updateTrail(float robotX, float robotY, float robotZ)
bool
addToTrail
=
false
;
if
(
trail
.
size
()
>
0
)
{
if
(
fabs
f
(
robotX
-
trail
[
trail
.
size
()
-
1
].
x
())
>
0.01
f
||
fabs
f
(
robotY
-
trail
[
trail
.
size
()
-
1
].
y
())
>
0.01
f
||
fabs
f
(
robotZ
-
trail
[
trail
.
size
()
-
1
].
z
())
>
0.01
f
)
if
(
fabs
(
robotX
-
trail
[
trail
.
size
()
-
1
].
x
())
>
0.01
f
||
fabs
(
robotY
-
trail
[
trail
.
size
()
-
1
].
y
())
>
0.01
f
||
fabs
(
robotZ
-
trail
[
trail
.
size
()
-
1
].
z
())
>
0.01
f
)
{
addToTrail
=
true
;
}
...
...
@@ -739,11 +752,11 @@ Pixhawk3DWidget::updateTrail(float robotX, float robotY, float robotZ)
if
(
addToTrail
)
{
osg
::
Vec3
p
(
robotX
,
robotY
,
robotZ
);
osg
::
Vec3
d
p
(
robotX
,
robotY
,
robotZ
);
if
(
trail
.
size
()
==
trail
.
capacity
())
{
memcpy
(
trail
.
data
(),
trail
.
data
()
+
1
,
(
trail
.
size
()
-
1
)
*
sizeof
(
osg
::
Vec3
));
(
trail
.
size
()
-
1
)
*
sizeof
(
osg
::
Vec3
d
));
trail
[
trail
.
size
()
-
1
]
=
p
;
}
else
...
...
@@ -755,9 +768,9 @@ Pixhawk3DWidget::updateTrail(float robotX, float robotY, float robotZ)
trailVertices
->
clear
();
for
(
int
i
=
0
;
i
<
trail
.
size
();
++
i
)
{
trailVertices
->
push_back
(
osg
::
Vec3
(
trail
[
i
].
y
()
-
robotY
,
trail
[
i
].
x
()
-
robotX
,
-
(
trail
[
i
].
z
()
-
robotZ
)));
trailVertices
->
push_back
(
osg
::
Vec3
d
(
trail
[
i
].
y
()
-
robotY
,
trail
[
i
].
x
()
-
robotX
,
-
(
trail
[
i
].
z
()
-
robotZ
)));
}
trailDrawArrays
->
setFirst
(
0
);
...
...
@@ -768,6 +781,11 @@ Pixhawk3DWidget::updateTrail(float robotX, float robotY, float robotZ)
void
Pixhawk3DWidget
::
updateImagery
(
void
)
{
if
(
mapNode
->
getImageryType
()
==
Imagery
::
BLANK_MAP
)
{
return
;
}
char
zone
[
5
]
=
"32T"
;
double
viewingRadius
=
cameraManipulator
->
getDistance
()
*
10.0
;
...
...
@@ -777,7 +795,7 @@ Pixhawk3DWidget::updateImagery(void)
}
double
minResolution
=
0.25
;
double
centerResolution
=
cameraManipulator
->
getDistance
()
/
25
.0
;
double
centerResolution
=
cameraManipulator
->
getDistance
()
/
50
.0
;
double
maxResolution
=
1048576.0
;
Imagery
::
ImageryType
imageryType
=
mapNode
->
getImageryType
();
...
...
@@ -1100,7 +1118,7 @@ Pixhawk3DWidget::updateRGBD(void)
void
Pixhawk3DWidget
::
markTarget
(
void
)
{
float
robotZ
=
0.0
f
;
double
robotZ
=
0.0
f
;
if
(
uas
!=
NULL
)
{
robotZ
=
uas
->
getLocalZ
();
...
...
src/ui/map3D/Pixhawk3DWidget.h
View file @
7e98c607
...
...
@@ -88,9 +88,9 @@ private:
void
setupHUD
(
void
);
void
resizeHUD
(
void
);
void
updateHUD
(
float
robotX
,
float
robotY
,
float
robotZ
,
float
robotRoll
,
float
robotPitch
,
float
robotYaw
);
void
updateTrail
(
float
robotX
,
float
robotY
,
float
robotZ
);
void
updateHUD
(
double
robotX
,
double
robotY
,
double
robotZ
,
double
robotRoll
,
double
robotPitch
,
double
robotYaw
);
void
updateTrail
(
double
robotX
,
double
robotY
,
double
robotZ
);
void
updateImagery
(
void
);
void
updateTarget
(
void
);
void
updateWaypoints
(
void
);
...
...
@@ -111,8 +111,8 @@ private:
bool
followCamera
;
osg
::
ref_ptr
<
osg
::
Vec3Array
>
trailVertices
;
QVarLengthArray
<
osg
::
Vec3
,
10000
>
trail
;
osg
::
ref_ptr
<
osg
::
Vec3
d
Array
>
trailVertices
;
QVarLengthArray
<
osg
::
Vec3
d
,
10000
>
trail
;
osg
::
ref_ptr
<
osg
::
Node
>
vehicleModel
;
osg
::
ref_ptr
<
osg
::
Geometry
>
hudBackgroundGeometry
;
...
...
@@ -134,6 +134,7 @@ private:
QScopedPointer
<
Freenect
>
freenect
;
QVector
<
Freenect
::
Vector6D
>
pointCloud
;
#endif
bool
enableFreenect
;
QSharedPointer
<
QByteArray
>
rgb
;
QSharedPointer
<
QByteArray
>
coloredDepth
;
...
...
@@ -141,7 +142,7 @@ private:
QPushButton
*
targetButton
;
float
lastRobotX
,
lastRobotY
,
lastRobotZ
;
double
lastRobotX
,
lastRobotY
,
lastRobotZ
;
};
#endif // PIXHAWK3DWIDGET_H
src/ui/map3D/Q3DWidget.cc
View file @
7e98c607
...
...
@@ -181,13 +181,13 @@ Q3DWidget::setCameraParams(float minZoomRange, float cameraFov,
}
void
Q3DWidget
::
moveCamera
(
float
dx
,
float
dy
,
float
dz
)
Q3DWidget
::
moveCamera
(
double
dx
,
double
dy
,
double
dz
)
{
cameraManipulator
->
move
(
dx
,
dy
,
dz
);
}
void
Q3DWidget
::
recenterCamera
(
float
x
,
float
y
,
float
z
)
Q3DWidget
::
recenterCamera
(
double
x
,
double
y
,
double
z
)
{
cameraManipulator
->
setCenter
(
osg
::
Vec3d
(
x
,
y
,
z
));
}
...
...
@@ -385,18 +385,6 @@ Q3DWidget::wheelEvent(QWheelEvent* event)
osgGA
::
GUIEventAdapter
::
SCROLL_DOWN
);
}
float
Q3DWidget
::
r2d
(
float
angle
)
{
return
angle
*
57.295779513082320876
f
;
}
float
Q3DWidget
::
d2r
(
float
angle
)
{
return
angle
*
0.0174532925199432957692
f
;
}
osgGA
::
GUIEventAdapter
::
KeySymbol
Q3DWidget
::
convertKey
(
int
key
)
const
{
...
...
src/ui/map3D/Q3DWidget.h
View file @
7e98c607
...
...
@@ -88,12 +88,12 @@ public:
* @param dy Translation along the y-axis in meters.
* @param dz Translation along the z-axis in meters.
*/
void
moveCamera
(
float
dx
,
float
dy
,
float
dz
);
void
moveCamera
(
double
dx
,
double
dy
,
double
dz
);
/**
* @brief Recenters the camera at (x,y,z).
*/
void
recenterCamera
(
float
x
,
float
y
,
float
z
);
void
recenterCamera
(
double
x
,
double
y
,
double
z
);
/**
* @brief Sets up 3D display mode.
...
...
@@ -211,20 +211,6 @@ protected:
*/
virtual
void
wheelEvent
(
QWheelEvent
*
event
);
/**
* @brief Converts radians to degrees.
* @param angle Angle in radians.
* @return angle in degrees.
*/
float
r2d
(
float
angle
);
/**
* @brief Converts degrees to radians.
* @param angle Angle in degrees.
* @return angle in radians.
*/
float
d2r
(
float
angle
);
/**
* @brief Converts Qt-defined key to OSG-defined key.
* @param key Qt-defined key.
...
...
src/ui/map3D/Texture.cc
View file @
7e98c607
...
...
@@ -47,7 +47,7 @@ Texture::Texture(unsigned int _id)
osg
::
ref_ptr
<
osg
::
Image
>
image
=
new
osg
::
Image
;
texture2D
->
setImage
(
image
);
osg
::
ref_ptr
<
osg
::
Vec
2Array
>
vertices
(
new
osg
::
Vec2
Array
(
4
));
osg
::
ref_ptr
<
osg
::
Vec
3dArray
>
vertices
(
new
osg
::
Vec3d
Array
(
4
));
geometry
->
setVertexArray
(
vertices
);
osg
::
ref_ptr
<
osg
::
Vec2Array
>
textureCoords
=
new
osg
::
Vec2Array
;
...
...
@@ -55,7 +55,7 @@ Texture::Texture(unsigned int _id)
textureCoords
->
push_back
(
osg
::
Vec2
(
1.0
f
,
1.0
f
));
textureCoords
->
push_back
(
osg
::
Vec2
(
1.0
f
,
0.0
f
));
textureCoords
->
push_back
(
osg
::
Vec2
(
0.0
f
,
0.0
f
));
geometry
->
setTexCoordArray
(
id
,
textureCoords
);
geometry
->
setTexCoordArray
(
0
,
textureCoords
);
geometry
->
addPrimitiveSet
(
new
osg
::
DrawArrays
(
osg
::
PrimitiveSet
::
LINES
,
0
,
4
));
...
...
@@ -66,6 +66,13 @@ Texture::Texture(unsigned int _id)
geometry
->
setColorBinding
(
osg
::
Geometry
::
BIND_OVERALL
);
geometry
->
setUseDisplayList
(
false
);
osg
::
ref_ptr
<
osg
::
LineWidth
>
linewidth
(
new
osg
::
LineWidth
);
linewidth
->
setWidth
(
2.0
f
);
geometry
->
getOrCreateStateSet
()
->
setAttributeAndModes
(
linewidth
,
osg
::
StateAttribute
::
ON
);
geometry
->
getOrCreateStateSet
()
->
setMode
(
GL_LIGHTING
,
osg
::
StateAttribute
::
OFF
);
}
const
QString
&
...
...
@@ -94,8 +101,8 @@ Texture::sync(const WebImagePtr& image)
texture2D
->
getImage
()
->
setImage
(
image
->
getWidth
(),
image
->
getHeight
(),
1
,
GL_RGB
,
GL_RGB
,
GL_RGB
A
,
GL_RGB
A
,
GL_UNSIGNED_BYTE
,
image
->
getImageData
(),
osg
::
Image
::
NO_DELETE
);
...
...
@@ -105,23 +112,23 @@ Texture::sync(const WebImagePtr& image)
}
osg
::
ref_ptr
<
osg
::
Geometry
>
Texture
::
draw
(
float
x1
,
float
y1
,
float
x2
,
float
y2
,
Texture
::
draw
(
double
x1
,
double
y1
,
double
x2
,
double
y2
,
bool
smoothInterpolation
)
const
{
return
draw
(
x1
,
y1
,
x2
,
y1
,
x2
,
y2
,
x1
,
y2
,
smoothInterpolation
);
}
osg
::
ref_ptr
<
osg
::
Geometry
>
Texture
::
draw
(
float
x1
,
float
y1
,
float
x2
,
float
y2
,
float
x3
,
float
y3
,
float
x4
,
float
y4
,
Texture
::
draw
(
double
x1
,
double
y1
,
double
x2
,
double
y2
,
double
x3
,
double
y3
,
double
x4
,
double
y4
,
bool
smoothInterpolation
)
const
{
osg
::
Vec
2
Array
*
vertices
=
static_cast
<
osg
::
Vec
2
Array
*>
(
geometry
->
getVertexArray
());
(
*
vertices
)[
0
].
set
(
x1
,
y1
);
(
*
vertices
)[
1
].
set
(
x2
,
y2
);
(
*
vertices
)[
2
].
set
(
x3
,
y3
);
(
*
vertices
)[
3
].
set
(
x4
,
y4
);
osg
::
Vec
3d
Array
*
vertices
=
static_cast
<
osg
::
Vec
3d
Array
*>
(
geometry
->
getVertexArray
());
(
*
vertices
)[
0
].
set
(
x1
,
y1
,
-
0.1
);
(
*
vertices
)[
1
].
set
(
x2
,
y2
,
-
0.1
);
(
*
vertices
)[
2
].
set
(
x3
,
y3
,
-
0.1
);
(
*
vertices
)[
3
].
set
(
x4
,
y4
,
-
0.1
);
osg
::
DrawArrays
*
drawarrays
=
static_cast
<
osg
::
DrawArrays
*>
(
geometry
->
getPrimitiveSet
(
0
));
...
...
@@ -130,11 +137,11 @@ Texture::draw(float x1, float y1, float x2, float y2,
if
(
state
==
REQUESTED
)
{
drawarrays
->
set
(
osg
::
PrimitiveSet
::
LINE
S
,
0
,
4
);
drawarrays
->
set
(
osg
::
PrimitiveSet
::
LINE
_LOOP
,
0
,
4
);
(
*
colors
)[
0
].
set
(
0.0
f
,
0.0
f
,
1.0
f
,
1.0
f
);
geometry
->
getOrCreateStateSet
()
->
setTextureAttributeAndModes
(
id
,
texture2D
,
osg
::
StateAttribute
::
OFF
);
setTextureAttributeAndModes
(
0
,
texture2D
,
osg
::
StateAttribute
::
OFF
);
return
geometry
;
}
...
...
@@ -154,7 +161,7 @@ Texture::draw(float x1, float y1, float x2, float y2,
(
*
colors
)[
0
].
set
(
1.0
f
,
1.0
f
,
1.0
f
,
1.0
f
);
geometry
->
getOrCreateStateSet
()
->
setTextureAttributeAndModes
(
id
,
texture2D
,
osg
::
StateAttribute
::
ON
);
setTextureAttributeAndModes
(
0
,
texture2D
,
osg
::
StateAttribute
::
ON
);
return
geometry
;
}
src/ui/map3D/Texture.h
View file @
7e98c607
...
...
@@ -51,10 +51,10 @@ public:
void
sync
(
const
WebImagePtr
&
image
);
osg
::
ref_ptr
<
osg
::
Geometry
>
draw
(
float
x1
,
float
y1
,
float
x2
,
float
y2
,
osg
::
ref_ptr
<
osg
::
Geometry
>
draw
(
double
x1
,
double
y1
,
double
x2
,
double
y2
,
bool
smoothInterpolation
)
const
;
osg
::
ref_ptr
<
osg
::
Geometry
>
draw
(
float
x1
,
float
y1
,
float
x2
,
float
y2
,
float
x3
,
float
y3
,
float
x4
,
float
y4
,
osg
::
ref_ptr
<
osg
::
Geometry
>
draw
(
double
x1
,
double
y1
,
double
x2
,
double
y2
,
double
x3
,
double
y3
,
double
x4
,
double
y4
,
bool
smoothInterpolation
)
const
;
private:
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment