第一人称操作器

#pragma once

#include<osgGA/CameraManipulator>
#include<osgViewer/Viewer>
#include <osg/PositionAttitudeTransform>

class FirstPersonManipulator :public osgGA::CameraManipulator
{
public:
	FirstPersonManipulator(osg::ref_ptr<osg::PositionAttitudeTransform> initial);
	~FirstPersonManipulator();
	virtual void setByMatrix(const osg::Matrixd& matrix){};
	virtual void setByInverseMatrix(const osg::Matrixd& matrix) {};
	virtual osg::Matrixd getMatrix() const;
	virtual osg::Matrixd getInverseMatrix() const;
	virtual bool handle(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& us);
 
	osg::Vec3 Screen2World(osgViewer::Viewer* view, float x, float y);
	bool ChangePosition(osg::Vec3 delta);
	void SetStep(int delta_step);
private:
	osg::Vec3 m_vPosition;
	osg::Vec3 m_vRotation;
	int m_vStep;
	float m_vRotateStep;
	int m_iLeftX;
	int m_iLeftY;
	bool m_bLeftDown;
};

#include "FirstPersonManipulator.h"
#include <osg/ComputeBoundsVisitor>
#include <iostream>
 
FirstPersonManipulator::FirstPersonManipulator(osg::ref_ptr<osg::PositionAttitudeTransform> initial)
{
	osg::ComputeBoundsVisitor cbv;
	initial->accept(cbv);
	osg::BoundingBox boundingBox = cbv.getBoundingBox();

	m_vPosition = boundingBox.center(); 
	m_vPosition[2] = boundingBox.radius();

	m_vRotation = osg::Vec3(0, 0, 0);
	m_vStep = 1.0;
	m_vRotateStep = 0.05;
	m_bLeftDown = false;
}

FirstPersonManipulator::~FirstPersonManipulator()
{
}

osg::Matrixd FirstPersonManipulator::getMatrix() const
{
	osg::Matrixd mat;
	mat.makeTranslate(m_vPosition);
	return osg::Matrixd::rotate(m_vRotation[0], osg::X_AXIS, m_vRotation[1], osg::Y_AXIS, m_vRotation[2], osg::Z_AXIS)*mat;
}
 
osg::Matrixd FirstPersonManipulator::getInverseMatrix() const
{
	osg::Matrixd mat;
	mat.makeTranslate(m_vPosition);
	return osg::Matrixd::inverse(osg::Matrixd::rotate(m_vRotation[0], osg::X_AXIS, m_vRotation[1], osg::Y_AXIS, m_vRotation[2], osg::Z_AXIS)*mat);
}
 
bool FirstPersonManipulator::handle(const osgGA::GUIEventAdapter &ea, osgGA::GUIActionAdapter &us)
{
	osgViewer::Viewer* view = dynamic_cast<osgViewer::Viewer*>(&us);
	switch (ea.getEventType())
	{
		case osgGA::GUIEventAdapter::KEYDOWN:
		{
			if (ea.getKey() == 'w' || ea.getKey() == 'W')
			{
				osg::Vec3 vNormalize = -Screen2World(view, ea.getX(), ea.getY());
				vNormalize *= m_vStep;
				ChangePosition(vNormalize);
			}
			else if (ea.getKey() == 's' || ea.getKey() == 'S')
			{
				osg::Vec3 vNormalize = Screen2World(view, ea.getX(), ea.getY());
				vNormalize *= m_vStep;
				ChangePosition(vNormalize);
			}
			else if (ea.getKey() == 'a' || ea.getKey() == 'A')
			{
				ChangePosition(osg::Vec3(-m_vStep * sinf(osg::PI_2 + m_vRotation._v[2]), m_vStep * cosf(osg::PI_2 + m_vRotation._v[2]), 0));
			}
			else if (ea.getKey() == 'd' || ea.getKey() == 'D')
			{
				ChangePosition(osg::Vec3(m_vStep * sinf(osg::PI_2 + m_vRotation._v[2]), -m_vStep * cosf(osg::PI_2 + m_vRotation._v[2]), 0));
			}	
			else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_KP_Add || ea.getKey() == osgGA::GUIEventAdapter::KEY_Equals)
			{
				SetStep(2);
			}
			else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_KP_Subtract || ea.getKey() == osgGA::GUIEventAdapter::KEY_Minus)
			{
				SetStep(-2);
			}
			else if (ea.getKey() == 'q' || ea.getKey() == 'Q')
			{
				ChangePosition(osg::Vec3(0.0, 0.0, 1));
			}
			else if (ea.getKey() == 'e' || ea.getKey() == 'E')
			{
				ChangePosition(osg::Vec3(0.0, 0.0, -1.0));
			}
		}
		break;
		case osgGA::GUIEventAdapter::SCROLL:
			if (ea.getScrollingMotion() == osgGA::GUIEventAdapter::SCROLL_DOWN)
			{
				osg::Vec3 vNormalize = -Screen2World(view, ea.getX(), ea.getY());
				vNormalize *= m_vStep;
				ChangePosition(vNormalize);
			}
			else if(ea.getScrollingMotion() == osgGA::GUIEventAdapter::SCROLL_UP)
			{
				osg::Vec3 vNormalize = Screen2World(view, ea.getX(), ea.getY());
				vNormalize *= m_vStep;
				ChangePosition(vNormalize);
			}
		break;
		case osgGA::GUIEventAdapter::PUSH:
		{
			if (ea.getButton() == osgGA::GUIEventAdapter::LEFT_MOUSE_BUTTON)
			{
				m_iLeftX = ea.getX();
				m_iLeftY = ea.getY();
				m_bLeftDown = true;
			}
		}
		break;
		case osgGA::GUIEventAdapter::DRAG:
		{
			if (m_bLeftDown)
			{
				int delX = ea.getX() - m_iLeftX;
				int delY = ea.getY() - m_iLeftY;
				m_vRotation[2] -= osg::DegreesToRadians(m_vRotateStep * delX);
				m_vRotation[0] += osg::DegreesToRadians(m_vRotateStep * delY);
				if (m_vRotation[0] <= 0)
				{
					m_vRotation[0] = 0;
				}
				if (m_vRotation[0] >= osg::PI)
				{
					m_vRotation[0] = osg::PI;
				}
				m_iLeftX = ea.getX();
				m_iLeftY = ea.getY();
				//std::cout << osg::RadiansToDegrees(m_vRotation[0]) << std::endl;
			}
		}
		break;
		case osgGA::GUIEventAdapter::RELEASE:
		{
			if (ea.getButton() == osgGA::GUIEventAdapter::LEFT_MOUSE_BUTTON)
			{
				m_bLeftDown = false;
			}
		}
		break;
		default:
		break;
	}
	return false;
}
 
bool FirstPersonManipulator::ChangePosition(osg::Vec3 delta)
{
	m_vPosition += delta;
	return true;
}

void FirstPersonManipulator::SetStep(int delta_step)
{
	m_vStep += delta_step;
	if (m_vStep <= 1)
		m_vStep = 1;
}

osg::Vec3 FirstPersonManipulator::Screen2World(osgViewer::Viewer* view, float x, float y)
{
	osg::ref_ptr<osg::Camera> camera = view->getCamera();

	//v_window = v_local * MVPW
	//v_local = v_window *  inverse(MVPW)
	//MVPW = ModelViewMatrix * ProjectionMatrix * WindowMatrix
	osg::Vec3 vWindow(x, y, 0);
	osg::Matrix mVPW = camera->getViewMatrix() * camera->getProjectionMatrix() * camera->getViewport()->computeWindowMatrix();
	osg::Matrix invertMVPW;
	invertMVPW.invert(mVPW);
	osg::Vec3 vLocal = vWindow * invertMVPW;

	//VView = VViewWorld * MView
	//VViewWorld = VView * inverse(MView)
	//VView = osg::Vec3(0, 0, 0)      MView = camera->getViewMatrix()
	osg::Vec3 vView = osg::Vec3(0, 0, 0);
	osg::Matrix mView = camera->getViewMatrix();
	osg::Matrix inverseMView;
	inverseMView.invert(camera->getViewMatrix());
	osg::Vec3 vViewWorld = vView * inverseMView;

	//std::cout << vLocal.x() << "  " << vLocal.y() << "  " << vLocal.z() << "  " << vViewWorld.x() << "  " << vViewWorld.y() << "  " << vViewWorld.z() << std::endl;
	osg::Vec3 vMouse = vViewWorld - vLocal;
	vMouse.normalize();
	return vMouse;
}