OSG绘制视锥体

最近要来实现一个相机位姿态可视化的需求，不想使用pangolin，不好集成，想用osg来做可视化。以下是demo效果。

代码实现：

// Cone_of_vision.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"
#include <osgViewer/Viewer>
#include <osg/Camera>
#include <osg/Geode>
#include <osg/Geometry>

// 在创建相机视锥体时计算视锥体顶点和设置边线
osg::ref_ptr<osg::Geode> createCameraFrustum(osg::Camera* camera) {
	osg::ref_ptr<osg::Geode> geode = new osg::Geode();
	osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry();

	// 获取相机的投影矩阵和视图矩阵
	osg::Matrixd projectionMatrix = camera->getProjectionMatrix();
	osg::Matrixd viewMatrix = camera->getViewMatrix();

	// 计算视锥体顶点坐标
	osg::Vec3Array* vertices = new osg::Vec3Array(9);
	double nearPlane, farPlane;
	// 获取近和远裁剪平面的值
	double fovY = 1;
	double aspectRatio = 0.5;
	projectionMatrix.getPerspective(fovY, aspectRatio, nearPlane, farPlane);
	farPlane = 1.9;
	double tanFovY = tan(fovY * 0.5);
	double tanFovX = tanFovY * aspectRatio;

	// 近裁剪平面的四个顶点
	(*vertices)[0] = osg::Vec3(0.0, 0.0, 0.0);
	(*vertices)[1] = osg::Vec3(tanFovX * nearPlane, tanFovY * nearPlane, -nearPlane);
	(*vertices)[2] = osg::Vec3(-tanFovX * nearPlane, tanFovY * nearPlane, -nearPlane);
	(*vertices)[3] = osg::Vec3(-tanFovX * nearPlane, -tanFovY * nearPlane, -nearPlane);
	(*vertices)[4] = osg::Vec3(tanFovX * nearPlane, -tanFovY * nearPlane, -nearPlane);

	// 远裁剪平面的四个顶点
	(*vertices)[5] = osg::Vec3(tanFovX * farPlane, tanFovY * farPlane, -farPlane);
	(*vertices)[6] = osg::Vec3(-tanFovX * farPlane, tanFovY * farPlane, -farPlane);
	(*vertices)[7] = osg::Vec3(-tanFovX * farPlane, -tanFovY * farPlane, -farPlane);
	(*vertices)[8] = osg::Vec3(tanFovX * farPlane, -tanFovY * farPlane, -farPlane);

	// 设置视锥体的边线
	osg::ref_ptr<osg::DrawElementsUInt> edges = new osg::DrawElementsUInt(osg::PrimitiveSet::LINES, 24);
	// 给edges数组添加顶点索引来定义边线
	edges->push_back(0); 
	edges->push_back(1);
	edges->push_back(0); 
	edges->push_back(2);
	edges->push_back(0);
	edges->push_back(3);
	edges->push_back(0);
	edges->push_back(4);

	edges->push_back(1);
	edges->push_back(2);
	edges->push_back(3);
	edges->push_back(4);

	edges->push_back(1);
	edges->push_back(4);
	edges->push_back(2);
	edges->push_back(3);


	edges->push_back(0 );
	edges->push_back(1 + 4);
	edges->push_back(0 );
	edges->push_back(2 + 4);
	edges->push_back(0 );
	edges->push_back(3 + 4);
	edges->push_back(0 );
	edges->push_back(4 + 4);
	edges->push_back(1 + 4);
	edges->push_back(2 + 4);
	edges->push_back(3 + 4);
	edges->push_back(4 + 4);

	edges->push_back(1 + 4);
	edges->push_back(4 + 4);
	edges->push_back(2 + 4);
	edges->push_back(3 + 4);

	/*edges->push_back(0);
	edges->push_back(7);
	edges->push_back(0);
	edges->push_back(8);*/

	// 其他边线的索引添加类似的操作...

	// 设置几何体属性
	geometry->setVertexArray(vertices);
	geometry->addPrimitiveSet(edges);

	geode->addDrawable(geometry);

	return geode;
}

osg::ref_ptr<osg::Geode> createPyramid() {
	osg::ref_ptr<osg::Geode> geode = new osg::Geode();
	osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry();

	// 顶点数组
	osg::ref_ptr<osg::Vec3Array> vertices = new osg::Vec3Array;
	vertices->push_back(osg::Vec3(0.0f, 0.0f, 1.0f));  // 顶点
	vertices->push_back(osg::Vec3(1.0f, 0.0f, -1.0f)); // 底面顶点1
	vertices->push_back(osg::Vec3(-1.0f, 0.0f, -1.0f)); // 底面顶点2
	vertices->push_back(osg::Vec3(0.0f, 1.0f, 0.0f)); // 底面顶点3
	vertices->push_back(osg::Vec3(0.0f, -1.0f, 0.0f)); // 底面顶点4

	// 设置几何体的顶点
	geometry->setVertexArray(vertices.get());

	// 底面索引数组
	osg::ref_ptr<osg::DrawElementsUInt> baseIndices = new osg::DrawElementsUInt(osg::PrimitiveSet::TRIANGLES);
	baseIndices->push_back(1);
	baseIndices->push_back(2);
	baseIndices->push_back(3);

	baseIndices->push_back(2);
	baseIndices->push_back(1);
	baseIndices->push_back(4);

	baseIndices->push_back(3);
	baseIndices->push_back(2);
	baseIndices->push_back(4);

	baseIndices->push_back(1);
	baseIndices->push_back(3);
	baseIndices->push_back(4);

	// 侧面索引数组
	osg::ref_ptr<osg::DrawElementsUInt> sideIndices = new osg::DrawElementsUInt(osg::PrimitiveSet::TRIANGLES);
	sideIndices->push_back(0);
	sideIndices->push_back(1);
	sideIndices->push_back(2);

	sideIndices->push_back(0);
	sideIndices->push_back(2);
	sideIndices->push_back(3);

	sideIndices->push_back(0);
	sideIndices->push_back(3);
	sideIndices->push_back(4);

	sideIndices->push_back(0);
	sideIndices->push_back(4);
	sideIndices->push_back(1);

	// 添加底面和侧面索引
	geometry->addPrimitiveSet(baseIndices.get());
	geometry->addPrimitiveSet(sideIndices.get());

	geode->addDrawable(geometry.get());

	return geode;
}

osg::ref_ptr <osg::Geode > cteateQuad()
{
	//创建一个叶节点对象
	osg::ref_ptr <osg::Geode > geode = new osg::Geode();
	//创建一个几何体对象
	osg::ref_ptr <osg::Geometry >geom = new osg::Geometry();
	//添加顶点数据 注意顶点的添加顺序是逆时针
	osg::ref_ptr <osg::Vec3Array >v = new osg::Vec3Array();
	//添加数据
	v->push_back(osg::Vec3(0, 0, 0));
	v->push_back(osg::Vec3(1, 0, 0));
	v->push_back(osg::Vec3(1, 0, 1));
	v->push_back(osg::Vec3(0, 0, 1));

	//设置顶点数据
	geom->setVertexArray(v.get());

	//创建纹理订点数据
	osg::ref_ptr <osg::Vec2Array >vt = new osg::Vec2Array();
	//添加纹理坐标
	vt->push_back(osg::Vec2(0, 0));
	vt->push_back(osg::Vec2(1, 0));
	vt->push_back(osg::Vec2(1, 1));
	vt->push_back(osg::Vec2(0, 1));

	//设置纹理坐标
	geom->setTexCoordArray(0, vt.get());

	//创建颜色数组
	osg::ref_ptr <osg::Vec4Array >vc = new osg::Vec4Array();
	//添加数据
	vc->push_back(osg::Vec4(1, 0, 0, 1));
	vc->push_back(osg::Vec4(0, 1, 0, 1));
	vc->push_back(osg::Vec4(0, 0, 1, 1));
	vc->push_back(osg::Vec4(1, 1, 0, 1));

	//设置颜色数组
	geom->setColorArray(vc.get());
	//设置颜色的绑定方式为单个顶点
	geom->setColorBinding(osg::Geometry::BIND_PER_VERTEX);

	//创建法线数组
	osg::ref_ptr <osg::Vec3Array >nc = new osg::Vec3Array();
	//添加法线
	nc->push_back(osg::Vec3(0, -1, 0));
	//设置法线
	geom->setNormalArray(nc.get());
	//设置法绑定为全部顶点
	geom->setNormalBinding(osg::Geometry::BIND_OVERALL);
	//添加图元
	geom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::QUADS, 0, 4));

	//添加到叶子节点
	geode->addDrawable(geom.get());

	return geode.get();
}


int main() {
	// 创建场景图和视图器
	osg::ref_ptr<osg::Group> root = new osg::Group();
	osgViewer::Viewer viewer;

	// 创建四棱锥体几何体
	osg::ref_ptr<osg::Geode> pyramid = createCameraFrustum(viewer.getCamera());
	// 将四棱锥体添加到场景图中
	root->addChild(pyramid);

	// 设置场景图到视图器并运行
	viewer.setSceneData(root);
	return viewer.run();
}

?

?