//以下计算需要换算成局部坐标系下
/*
假设球机摄像头坐标为pos1(x1, y1, z1),枪机上拾取的点坐标为pos2(x2, y2, z2);
如果换算成局部坐标系,把球机摄像头作为原点O的话,那么球机摄像头pos1(0,0,0),枪机pos2(x2-x1, y2-y1, z2-z1);
再传入TwoPointsAngle函数中,计算出球机的转向
*/
#ifndef _VEC3D
#define _VEC3D 1
class Vec3d
{
public:
/** Data type of vector components.*/
typedef double value_type;
/** Number of vector components. */
enum { num_components = 3 };
value_type _v[3];
/** Constructor that sets all components of the vector to zero */
Vec3d() { _v[0]=0.0; _v[1]=0.0; _v[2]=0.0;}
inline Vec3d(const Vec3d& vec) { _v[0]=vec._v[0]; _v[1]=vec._v[1]; _v[2]=vec._v[2];}
Vec3d(value_type x,value_type y,value_type z) { _v[0]=x; _v[1]=y; _v[2]=z; }
inline bool operator == (const Vec3d& v) const { return _v[0]==v._v[0] && _v[1]==v._v[1] && _v[2]==v._v[2]; }
inline bool operator != (const Vec3d& v) const { return _v[0]!=v._v[0] || _v[1]!=v._v[1] || _v[2]!=v._v[2]; }
inline bool operator < (const Vec3d& v) const
{
if (_v[0]<v._v[0]) return true;
else if (_v[0]>v._v[0]) return false;
else if (_v[1]<v._v[1]) return true;
else if (_v[1]>v._v[1]) return false;
else return (_v[2]<v._v[2]);
}
inline value_type* ptr() { return _v; }
inline const value_type* ptr() const { return _v; }
inline void set( value_type x, value_type y, value_type z)
{
_v[0]=x; _v[1]=y; _v[2]=z;
}
inline void set( const Vec3d& rhs)
{
_v[0]=rhs._v[0]; _v[1]=rhs._v[1]; _v[2]=rhs._v[2];
}
inline value_type& operator [] (int i) { return _v[i]; }
inline value_type operator [] (int i) const { return _v[i]; }
inline value_type& x() { return _v[0]; }
inline value_type& y() { return _v[1]; }
inline value_type& z() { return _v[2]; }
inline value_type x() const { return _v[0]; }
inline value_type y() const { return _v[1]; }
inline value_type z() const { return _v[2]; }
/** Returns true if all components have values that are not NaN. */
inline bool valid() const { return !isNaN(); }
/** Returns true if at least one component has value NaN. */
inline bool isNaN() const { return osg::isNaN(_v[0]) || osg::isNaN(_v[1]) || osg::isNaN(_v[2]); }
/** Dot product. */
inline value_type operator * (const Vec3d& rhs) const
{
return _v[0]*rhs._v[0]+_v[1]*rhs._v[1]+_v[2]*rhs._v[2];
}
/** Cross product. */
inline const Vec3d operator ^ (const Vec3d& rhs) const
{
return Vec3d(_v[1]*rhs._v[2]-_v[2]*rhs._v[1],
_v[2]*rhs._v[0]-_v[0]*rhs._v[2] ,
_v[0]*rhs._v[1]-_v[1]*rhs._v[0]);
}
/** Multiply by scalar. */
inline const Vec3d operator * (value_type rhs) const
{
return Vec3d(_v[0]*rhs, _v[1]*rhs, _v[2]*rhs);
}
/** Unary multiply by scalar. */
inline Vec3d& operator *= (value_type rhs)
{
_v[0]*=rhs;
_v[1]*=rhs;
_v[2]*=rhs;
return *this;
}
/** Divide by scalar. */
inline const Vec3d operator / (value_type rhs) const
{
return Vec3d(_v[0]/rhs, _v[1]/rhs, _v[2]/rhs);
}
/** Unary divide by scalar. */
inline Vec3d& operator /= (value_type rhs)
{
_v[0]/=rhs;
_v[1]/=rhs;
_v[2]/=rhs;
return *this;
}
/** Binary vector add. */
inline const Vec3d operator + (const Vec3d& rhs) const
{
return Vec3d(_v[0]+rhs._v[0], _v[1]+rhs._v[1], _v[2]+rhs._v[2]);
}
/** Unary vector add. Slightly more efficient because no temporary
* intermediate object.
*/
inline Vec3d& operator += (const Vec3d& rhs)
{
_v[0] += rhs._v[0];
_v[1] += rhs._v[1];
_v[2] += rhs._v[2];
return *this;
}
/** Binary vector subtract. */
inline const Vec3d operator - (const Vec3d& rhs) const
{
return Vec3d(_v[0]-rhs._v[0], _v[1]-rhs._v[1], _v[2]-rhs._v[2]);
}
/** Unary vector subtract. */
inline Vec3d& operator -= (const Vec3d& rhs)
{
_v[0]-=rhs._v[0];
_v[1]-=rhs._v[1];
_v[2]-=rhs._v[2];
return *this;
}
/** Negation operator. Returns the negative of the Vec3d. */
inline const Vec3d operator - () const
{
return Vec3d (-_v[0], -_v[1], -_v[2]);
}
/** Length of the vector = sqrt( vec . vec ) */
inline value_type length() const
{
return sqrt( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] );
}
/** Length squared of the vector = vec . vec */
inline value_type length2() const
{
return _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2];
}
/** Normalize the vector so that it has length unity.
* Returns the previous length of the vector.
* If the vector is zero length, it is left unchanged and zero is returned.
*/
inline value_type normalize()
{
value_type norm = Vec3d::length();
if (norm>0.0)
{
value_type inv = 1.0/norm;
_v[0] *= inv;
_v[1] *= inv;
_v[2] *= inv;
}
return( norm );
}
}; // end of class Vec3d
/** multiply by vector components. */
inline Vec3d componentMultiply(const Vec3d& lhs, const Vec3d& rhs)
{
return Vec3d(lhs[0]*rhs[0], lhs[1]*rhs[1], lhs[2]*rhs[2]);
}
/** divide rhs components by rhs vector components. */
inline Vec3d componentDivide(const Vec3d& lhs, const Vec3d& rhs)
{
return Vec3d(lhs[0]/rhs[0], lhs[1]/rhs[1], lhs[2]/rhs[2]);
}
#endif //_VEC3D
//以下计算需要换算成局部坐标系下
/*
假设球机摄像头坐标为pos1(x1, y1, z1),枪机上拾取的点坐标为pos2(x2, y2, z2);
如果换算成局部坐标系,把球机摄像头作为原点O的话,那么球机摄像头pos1(0,0,0),枪机pos2(x2-x1, y2-y1, z2-z1);
再传入TwoPointsAngle函数中,计算出球机的转向
*/
#include "Vec3d.h"
#include <math.h>
void TwoPointsAngle(const Vec3d &pos1, const Vec3d &pos2, double& heading, double& pitch)
{
/*
A1(OA点放在一个XOY平面)
/ |
/ |
/ |
O—---B(A在+X轴的投影)
*/
//计算朝向OB到OA1向量夹角
{
Vec3d PO = pos1;
Vec3d PA = pos2;
PA._v[2] = PO._v[2];//OA点放在一个XOY平面
//osg::Vec3d PB(PA._x, PO._y, PA._z);//OB向量为+X轴方向
Vec3d PB(PO._v[0] + 1.0, 0.0, PO._v[2]);//OB向量为+X轴方向
Vec3d u = (PB - PO);
Vec3d v = (PA - PO);
u.normalize();
v.normalize();
double costheta = u*v / (sqrt(u._v[0] * u._v[0] + u._v[1] * u._v[1] + u._v[2] * u._v[2]) *sqrt(v._v[0] * v._v[0] + v._v[1] * v._v[1] + v._v[2] * v._v[2]));
heading = acos(costheta);
//heading
if (pos2.x() >= 0.0)
{
if (pos2.y() >= 0)
{
heading = heading;
}
else {
heading = osg::PI * 2 - heading;
}
}
else {
if (pos2.y() >= 0) {
heading = heading;
}
else {
heading = osg::PI * 2 - heading;
}
}
}
/*
A
/ |
/ |
/ |
O—---B(A在XOY面的投影)
*/
//计算俯仰OB到OA向量夹角
{
Vec3d PO = pos1;
Vec3d PA = pos2;
Vec3d PB(PA._v[0], PA._v[1], PO._v[2]);
PB._v[2] = PO._v[2];//OB放在一个平面,XOY平面
Vec3d PO_xyz = PO;
Vec3d PA_xyz = PA;
Vec3d PB_xyz = PB;
Vec3d u = (PB_xyz - PO_xyz);
Vec3d v = (PA_xyz - PO_xyz);
u.normalize();
v.normalize();
double costheta = u*v / (sqrt(u._v[0] * u._v[0] + u._v[1] * u._v[1] + u._v[2] * u._v[2]) *sqrt(v._v[0] * v._v[0] + v._v[1] * v._v[1] + v._v[2] * v._v[2]));
pitch = acos(costheta);
//pitch
if (pos2.z() < 0.0)
{
pitch = pitch;
}
else {
pitch = -pitch;
}
}
}