float complex
casinhf(float complex z)
{
float x, y, ax, ay, rx, ry, B, sqrt_A2my2, new_y;
int B_is_usable;
float complex w;
x = crealf(z);
y = cimagf(z);
ax = fabsf(x);
ay = fabsf(y);
if (isnan(x) || isnan(y)) {
if (isinf(x))
return (cpackf(x, y + y));
if (isinf(y))
return (cpackf(y, x + x));
if (y == 0)
return (cpackf(x + x, y));
return (cpackf(x + 0.0L + (y + 0), x + 0.0L + (y + 0)));
}
if (ax > RECIP_EPSILON || ay > RECIP_EPSILON) {
if (signbit(x) == 0)
w = clog_for_large_values(z) + m_ln2;
else
w = clog_for_large_values(-z) + m_ln2;
return (cpackf(copysignf(crealf(w), x),
copysignf(cimagf(w), y)));
}
if (x == 0 && y == 0)
return (z);
raise_inexact();
if (ax < SQRT_6_EPSILON / 4 && ay < SQRT_6_EPSILON / 4)
return (z);
do_hard_work(ax, ay, &rx, &B_is_usable, &B, &sqrt_A2my2, &new_y);
if (B_is_usable)
ry = asinf(B);
else
ry = atan2f(new_y, sqrt_A2my2);
return (cpackf(copysignf(rx, x), copysignf(ry, y)));
}
/*-----------------------------------------------------------------------------*\
* 从四元数转换为欧拉角表示 done
*
* out q的欧拉角表示
* q 源四元数
*-----------------------------------------------------------------------------*/
void magicalEulerAnglesFromQuaternion( cEulerAngles* out, const cQuaternion* q )
{
float sp = -2.0f * ( q->y * q->z + q->w * q->x );
if( fabsf( sp ) > 0.9999f )
{
out->pitch = kPIOver2 * sp;
out->yaw = atan2f( -q->x * q->z - q->w * q->y, 0.5f - q->y * q->y - q->z * q->z );
out->roll = 0.0f;
}
else
{
out->pitch = asinf( sp );
out->yaw = atan2f( q->x * q->z - q->w * q->y, 0.5f - q->x * q->x - q->y * q->y );
out->roll = atan2f( q->x * q->y - q->w * q->z, 0.5f - q->x * q->x - q->z * q->z );
}
}
inline void FXGLVertices::renderLines(FXGLViewer *viewer, bool isHit, bool complex){
#ifdef HAVE_GL_H
FXGLColor col(color);
GLUquadricObj* quad=0;
if(complex){
quad=gluNewQuadric();
gluQuadricDrawStyle(quad,(GLenum)GLU_FILL);
}
FXuint inc=(!(options & (SHADING_SMOOTH|SHADING_FLAT)) && viewer->doesTurbo()) ? 4 : 1;
for(FXuint n=0; n<vertexNumber-complex; n+=inc){
if(!isHit){
if(colorGenerator) for(FXuint c=0; c<inc; c++) colorGenerator(col, this, viewer, n, colorGeneratorData);
if(complex && (options & (SHADING_SMOOTH|SHADING_FLAT))){
glMaterialfv(GL_FRONT_AND_BACK,GL_DIFFUSE,&col.r);
}
else
glColor4fv(&col.r);
}
if(complex){
FXVec3f vector=vertices[n+1]-vertices[n];
FXfloat len=vector.length();
if(!(options & VERTICES_POINTS) || len>=pointSize*0.8f){
glPushMatrix();
glTranslatef(vertices[n].x,vertices[n].y,vertices[n].z);
FXVec3f zaxis(0.0f,0.0f,vector.z<0 ? 1.0f : -1.0f);
// Cylinder points in Z direction, so we need to rotate so it follows vector
FXVec3f vectornormal(vecnormalize(vector));
FXVec3f axis(vectornormal^zaxis);
FXfloat angle=((FXfloat) RTOD)*asinf(axis.length());
//FXfloat dotproduct=vectornormal*axis;
if(vector.z<0) angle+=180;
glRotatef(angle,axis.x,axis.y,axis.z);
gluCylinder(quad,lineSize/2,lineSize/2,len,8,8);
glPopMatrix();
}
}
else{
glVertex3fv(&vertices[n].x);
}
}
if(complex){
gluDeleteQuadric(quad);
}
#endif
}
void Quaternion::toRollPitchYaw(float rpy[3])
{
float R13, R11, R12, R23, R33;
float q0s = _w * _w;
float q1s = _x * _x;
float q2s = _y * _y;
float q3s = _z * _z;
R13 = 2.0 * (_x * _z - _w * _y);
R11 = q0s + q1s - q2s - q3s;
R12 = 2.0 * (_x * _y + _w * _z);
R23 = 2.0 * (_y * _z + _w * _x);
R33 = q0s - q1s - q2s + q3s;
rpy[1] = asinf(-R13); // pitch always between -pi/2 to pi/2
rpy[2] = atan2f(R12, R11);
rpy[0] = atan2f(R23, R33);
}
inline float ASin(float _x)
{
#if USE_CRT_MATH
return asinf(_x);
#else
//return ATan2(_x, Sqrt(1 - _x * _x));
float _r;
__asm fld _x
__asm fld _x
__asm fmul st(0), st(0)
__asm fld1
__asm fsubr
__asm fsqrt
__asm fpatan
__asm fstp _r
return _r;
#endif
}
/* Update the normal vector in vertex b of triangle (a,b,c).
* This corresponds to adding the normalized ab * bc vector product,
* multiplied by the angle in b */
void vertex::update_norm(const vertex &a, const vertex &b, const vertex &c) {
vertex v1, v2, p;
float norm, surf, angle;
v1 = b-a;
v2 = c-b;
p = v1*v2;
surf = sqrtf((v1.x * v1.x + v1.y * v1.y + v1.z * v1.z)
* (v2.x * v2.x + v2.y * v2.y + v2.z * v2.z));
norm = sqrtf(p.x * p.x + p.y * p.y + p.z * p.z);
if (norm > surf)
angle = M_PI / 2;
else
angle = asinf(norm / surf);
angle /= norm;
x += p.x * angle;
y += p.y * angle;
z += p.z * angle;
}
// Euler degress(X->Y->Z) <= matrix[4][4]
void CRMatrixMat4x42UnityEulerZXYDegrees3(float *degrees, float matrix[4][4]) {
float x = asinf(-matrix[1][2]);
float y = 0;
float z = 0;
if (cos(x) == 0) {
y = atan2f(-matrix[2][0], matrix[0][0]);
z = 0;
}
else {
y = atan2f(matrix[0][2], matrix[2][2]);
z = atan2f(matrix[1][0], matrix[1][1]);
}
degrees[0] = x;
degrees[1] = -y;
degrees[2] = -z;
}
/**
* graphene_quaternion_to_angles:
* @q: a #graphene_quaternion_t
* @deg_x: (out) (optional): return location for the rotation angle on
* the X axis (yaw), in degress
* @deg_y: (out) (optional): return location for the rotation angle on
* the Y axis (pitch), in degrees
* @deg_z: (out) (optional): return location for the rotation angle on
* the Z axis (roll), in degrees
*
* Converts a #graphene_quaternion_t to its corresponding rotations
* on the [Euler angles](http://en.wikipedia.org/wiki/Euler_angles)
* on each axis.
*
* Since: 1.2
*/
void
graphene_quaternion_to_angles (const graphene_quaternion_t *q,
float *deg_x,
float *deg_y,
float *deg_z)
{
graphene_vec4_t v;
graphene_vec4_t sq;
float qx, qy, qz, qw, sqx, sqy, sqz, sqw;
graphene_quaternion_to_vec4 (q, &v);
graphene_vec4_multiply (&v, &v, &sq);
qx = graphene_vec4_get_x (&v);
qy = graphene_vec4_get_y (&v);
qz = graphene_vec4_get_z (&v);
qw = graphene_vec4_get_w (&v);
sqx = graphene_vec4_get_x (&sq);
sqy = graphene_vec4_get_y (&sq);
sqz = graphene_vec4_get_z (&sq);
sqw = graphene_vec4_get_w (&sq);
if (deg_x != NULL)
{
float res = atan2f (2 * (qx * qw - qy * qz), (sqw - sqx - sqy + sqz));
*deg_x = GRAPHENE_RAD_TO_DEG (res);
}
if (deg_y != NULL)
{
float res = asinf (CLAMP (2 * ( qx * qz + qy * qw), -1, 1));
*deg_y = GRAPHENE_RAD_TO_DEG (res);
}
if (deg_z != NULL)
{
float res = atan2f (2 * (qz * qw - qx * qy), (sqw + sqx - sqy - sqz));
*deg_z = GRAPHENE_RAD_TO_DEG (res);
}
}
// Followed this tutorial: http://www.binpress.com/tutorial/creating-an-octahedron-sphere/162
MeshData MeshBuilder::CreateSphere(float radius, uint numSubdivisions, Color color)
{
MeshData data;
MeshVertex toAdd;
numSubdivisions = std::max(0u, std::min(numSubdivisions, 7u));
int resolution = 1 << numSubdivisions;
data.vertices.resize((resolution + 1) * (resolution + 1) * 4 - (resolution * 2 - 1) * 3);
data.indices.resize((1 << (numSubdivisions * 2 + 3)) * 3);
CreateOctahedron(data.vertices, data.indices, resolution);
for (uint i = 0; i < data.vertices.size(); ++i)
{
// Color
data.vertices[i].color = color;
// Position and Normals
data.vertices[i].position.Normalize();
data.vertices[i].normal = data.vertices[i].position;
Vector3 p = Vector3::Multiply(data.vertices[i].position, radius);
data.vertices[i].position = p;
// UVs
float theta = atan2(data.vertices[i].position.x, data.vertices[i].position.z);
float phi = asinf(data.vertices[i].position.y);
data.vertices[i].texcoord.x = theta / PI * 2;
if (data.vertices[i].texcoord.x < 0.0f)
data.vertices[i].texcoord.x += 1.0f;
data.vertices[i].texcoord.y = phi / PI + 0.5f;
// Tangents
data.vertices[i].tangent.x = -radius*sinf(phi)*sinf(theta);
data.vertices[i].tangent.y = 0.0f;
data.vertices[i].tangent.z = +radius*cosf(phi)*cosf(theta);
data.vertices[i].tangent.Normalize();
}
return data;
}
void
AltAzToRADec(long when, float lon, float lat, float alt, float az, float *ra, float *dec)
{
float ha_rad;
float lat_rad = lat * deg2rad;
float alt_rad = alt * deg2rad;
float az_rad = az * deg2rad;
float sin_lat = sinf(lat_rad);
float cos_lat = cosf(lat_rad);
float sin_alt = sinf(alt_rad);
float cos_alt = cosf(alt_rad);
float sin_az = sinf(az_rad);
float cos_az = cosf(az_rad);
float sin_dec = sin_alt * sin_lat + cos_alt * cos_lat * cos_az;
float dec_rad = asinf(sin_dec);
float cos_dec = cosf(dec_rad);
*dec = dec_rad * rad2deg;
float x = (sin_alt - sin_lat * sin_dec) / (cos_lat * cos_dec);
if (x < -1.0) {
ha_rad = pi;
} else if (x > 1.0) {
ha_rad = 0.0;
} else {
ha_rad = acosf(x);
}
if (sin_az > 0.0) {
ha_rad = (2.0 * pi) - ha_rad;
}
CDateTime dt(when);
*ra = dt.GetLocalSidereal(lon) - (ha_rad * rad2deg);
while (*ra < 0.0) {
*ra += 360.0;
}
while (*ra >= 360.0) {
*ra -= 360.0;
}
}
static TACommandVerdict asinf_cmd(TAThread thread,TAInputStream stream)
{
float x, res;
x = readFloat(&stream);
START_TARGET_OPERATION(thread);
errno = 0;
res = asinf(x);
END_TARGET_OPERATION(thread);
writeInt(thread, errno);
writeFloat(thread, res);
sendResponse(thread);
return taDefaultVerdict;
}
void Matrix4::toHeadPitchRoll(float &headDegrees, float &pitchDegrees, float &rollDegrees) const
{
// Extracts the Euler angles from a rotation matrix. The returned
// angles are in degrees. This method might suffer from numerical
// imprecision for ill defined rotation matrices.
//
// This function only works for rotation matrices constructed using
// the popular NASA standard airplane convention of heading-pitch-roll
// (i.e., RzRxRy).
//
// The algorithm used is from:
// David Eberly, "Euler Angle Formulas", Geometric Tools web site,
// http://www.geometrictools.com/Documentation/EulerAngles.pdf.
float thetaX = asinf(mtx[1][2]);
float thetaY = 0.0f;
float thetaZ = 0.0f;
if (thetaX < Math::HALF_PI)
{
if (thetaX > -Math::HALF_PI)
{
thetaZ = atan2f(-mtx[1][0], mtx[1][1]);
thetaY = atan2f(-mtx[0][2], mtx[2][2]);
}
else
{
// Not a unique solution.
thetaZ = -atan2f(mtx[2][0], mtx[0][0]);
thetaY = 0.0f;
}
}
else
{
// Not a unique solution.
thetaZ = atan2f(mtx[2][0], mtx[0][0]);
thetaY = 0.0f;
}
headDegrees = Math::radiansToDegrees(thetaY);
pitchDegrees = Math::radiansToDegrees(thetaX);
rollDegrees = Math::radiansToDegrees(thetaZ);
}
inline
void PaniniPortraitProjector::mapForward(float x, float y, float &u, float &v)
{
float y_ = r_kinv[0] * x + r_kinv[1] * y + r_kinv[2];
float x_ = r_kinv[3] * x + r_kinv[4] * y + r_kinv[5];
float z_ = r_kinv[6] * x + r_kinv[7] * y + r_kinv[8];
float u_ = atan2f(x_, z_);
float v_ = asinf(y_ / sqrtf(x_ * x_ + y_ * y_ + z_ * z_));
float tg = a * tanf(u_ / a);
u = - scale * tg;
float sinu = sinf( u_ );
if ( fabs(sinu) < 1E-7 )
v = scale * b * tanf(v_);
else
v = scale * b * tg * tanf(v_) / sinu;
}
float AngularDistance(float ra1, float dec1, float ra2, float dec2)
{
float ra1_rad = ra1 * deg2rad;
float ra2_rad = ra2 * deg2rad;
float dec1_rad = dec1 * deg2rad;
float dec2_rad = dec2 * deg2rad;
float dra = ra1_rad - ra2_rad;
float ddec = dec1_rad - dec2_rad;
float sin_ra = sinf(dra * 0.5);
float sin_dec = sinf(ddec * 0.5);
float sqr_ra = sin_ra * sin_ra;
float sqr_dec = sin_dec * sin_dec;
float aux = sqr_dec + cosf(dec1_rad) * cosf(dec2_rad) * sqr_ra;
return (2.0 * fabsf(asinf(sqrtf(aux)) * rad2deg));
}
void MayaCamera::LookAt(float3 posCamera, float3 posTarget)
{
m_posTarget = posTarget;
m_pos = posCamera;
// Calculate angles to look at posTarget
float3 vecToTarget = posTarget - posCamera;
ASSERT_WARN(!all(isnear(vecToTarget, 0.0f)));
m_radius = length(vecToTarget);
vecToTarget /= m_radius;
m_yaw = atan2f(-vecToTarget.z, vecToTarget.x);
m_pitch = asinf(vecToTarget.y);
// Update matrices
UpdateOrientation();
m_pos = m_posTarget + m_radius * m_viewToWorld[2].xyz;
setTranslation(&m_viewToWorld, m_pos);
UpdateWorldToClip();
}
void
RangeImagePlanar::setDepthImage (const unsigned short* depth_image, int di_width, int di_height,
float di_center_x, float di_center_y,
float di_focal_length_x, float di_focal_length_y,
float desired_angular_resolution)
{
//MEASURE_FUNCTION_TIME;
reset ();
float original_angular_resolution = asinf (0.5f*static_cast<float> (di_width)/static_cast<float> (di_focal_length_x)) / (0.5f*static_cast<float> (di_width));
int skip = 1;
if (desired_angular_resolution >= 2.0f*original_angular_resolution)
skip = static_cast<int> (pcl_lrint (floor (desired_angular_resolution/original_angular_resolution)));
setAngularResolution (original_angular_resolution * static_cast<float> (skip));
width = di_width / skip;
height = di_height / skip;
focal_length_x_ = di_focal_length_x / static_cast<float> (skip);
focal_length_x_reciprocal_ = 1.0f / focal_length_x_;
focal_length_y_ = di_focal_length_y / static_cast<float> (skip);
focal_length_y_reciprocal_ = 1.0f / focal_length_y_;
center_x_ = static_cast<float> (di_center_x) / static_cast<float> (skip);
center_y_ = static_cast<float> (di_center_y) / static_cast<float> (skip);
points.resize (width * height);
for (int y = 0; y < static_cast<int> (height); ++y)
{
for (int x = 0; x < static_cast<int> (width); ++x)
{
PointWithRange& point = getPointNoCheck (x, y);
float depth = depth_image[ (y*skip)*di_width + x*skip] * 0.001f;
if (depth <= 0.0f || !pcl_isfinite (depth))
{
point = unobserved_point;
continue;
}
point.z = depth;
point.x = (static_cast<float> (x) - center_x_) * point.z * focal_length_x_reciprocal_;
point.y = (static_cast<float> (y) - center_y_) * point.z * focal_length_y_reciprocal_;
point.range = point.getVector3fMap ().norm ();
}
}
}
bool Matrix44::getXYZ(Float* euler) const
{
// Code adapted from www.geometrictools.com
// Matrix3<Real>::EulerResult Matrix3<Real>::ToEulerAnglesXYZ
// +- -+ +- -+
// | r00 r01 r02 | | cy*cz -cy*sz sy |
// | r10 r11 r12 | = | cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx |
// | r20 r21 r22 | | -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy |
// +- -+ +- -+
if (_13 < 1.0f)
{
if (_13 > - 1.0f)
{
// y_angle = asin(r02)
// x_angle = atan2(-r12,r22)
// z_angle = atan2(-r01,r00)
euler[1] = asinf(_13);
euler[0] = atan2f(-_23,_33);
euler[2] = atan2f(-_12,_11);
return true;
}
else
{
// y_angle = -pi/2
// z_angle - x_angle = atan2(r10,r11)
// WARNING. The solution is not unique. Choosing z_angle = 0.
euler[1] = (Float)-M_PI_2;
euler[0] = -atan2f(_21,_22);
euler[2] = 0.0f;
return false;
}
}
else
{
// y_angle = +pi/2
// z_angle + x_angle = atan2(r10,r11)
// WARNING. The solutions is not unique. Choosing z_angle = 0.
euler[1] = (Float)M_PI_2;
euler[0] = atan2f(_21,_22);
euler[2] = 0.0f;
}
return false;
}
void CCustomMonster::mk_rotation (Fvector &dir, SRotation &R)
{
// parse yaw
Fvector DYaw;
DYaw.set (dir.x,0.f,dir.z);
DYaw.normalize_safe ();
clamp (DYaw.x,-0.9999999f,0.9999999f);
clamp (DYaw.y,-0.9999999f,0.9999999f);
clamp (DYaw.z,-0.9999999f,0.9999999f);
if ( DYaw.x >= 0 )
R.yaw = acosf(DYaw.z);
else
R.yaw = 2*PI-acosf(DYaw.z);
// parse pitch
dir.normalize_safe ();
R.pitch = -asinf(dir.y);
}
//-----------------------------------------------------------------------------
// Name: D3DUtil_GetRotationFromCursor()
// Desc: Returns a quaternion for the rotation implied by the window's cursor
// position.
//-----------------------------------------------------------------------------
D3DXQUATERNION D3DUtil_GetRotationFromCursor( HWND hWnd,
FLOAT fTrackBallRadius )
{
POINT pt;
RECT rc;
GetCursorPos( &pt );
GetClientRect( hWnd, &rc );
ScreenToClient( hWnd, &pt );
FLOAT sx = ( ( ( 2.0f * pt.x ) / (rc.right-rc.left) ) - 1 );
FLOAT sy = ( ( ( 2.0f * pt.y ) / (rc.bottom-rc.top) ) - 1 );
FLOAT sz;
if( sx == 0.0f && sy == 0.0f )
return D3DXQUATERNION( 0.0f, 0.0f, 0.0f, 1.0f );
FLOAT d2 = sqrtf( sx*sx + sy*sy );
if( d2 < fTrackBallRadius * 0.70710678118654752440 ) // Inside sphere
sz = sqrtf( fTrackBallRadius*fTrackBallRadius - d2*d2 );
else // On hyperbola
sz = (fTrackBallRadius*fTrackBallRadius) / (2.0f*d2);
// Get two points on trackball's sphere
D3DXVECTOR3 p1( sx, sy, sz );
D3DXVECTOR3 p2( 0.0f, 0.0f, fTrackBallRadius );
// Get axis of rotation, which is cross product of p1 and p2
D3DXVECTOR3 vAxis;
D3DXVec3Cross( &vAxis, &p1, &p2);
// Calculate angle for the rotation about that axis
D3DXVECTOR3 vecDiff = p2-p1;
FLOAT t = D3DXVec3Length( &vecDiff ) / ( 2.0f*fTrackBallRadius );
if( t > +1.0f) t = +1.0f;
if( t < -1.0f) t = -1.0f;
FLOAT fAngle = 2.0f * asinf( t );
// Convert axis to quaternion
D3DXQUATERNION quat;
D3DXQuaternionRotationAxis( &quat, &vAxis, fAngle );
return quat;
}
void MPLSensor::calcOrientationSensor(float *R, float *values)
{
float tmp;
//Azimuth
if ((R[7] > 0.7071067f) || ((R[8] < 0) && (fabs(R[7]) > fabs(R[6])))) {
values[0] = (float) atan2f(-R[3], R[0]);
} else {
values[0] = (float) atan2f(R[1], R[4]);
}
values[0] *= 57.295779513082320876798154814105f;
if (values[0] < 0) {
values[0] += 360.0f;
}
//Pitch
tmp = R[7];
if (tmp > 1.0f)
tmp = 1.0f;
if (tmp < -1.0f)
tmp = -1.0f;
values[1] = -asinf(tmp) * 57.295779513082320876798154814105f;
if (R[8] < 0) {
values[1] = 180.0f - values[1];
}
if (values[1] > 180.0f) {
values[1] -= 360.0f;
}
//Roll
if ((R[7] > 0.7071067f)) {
values[2] = (float) atan2f(R[6], R[7]);
} else {
values[2] = (float) atan2f(R[6], R[8]);
}
values[2] *= 57.295779513082320876798154814105f;
if (values[2] > 90.0f) {
values[2] = 180.0f - values[2];
}
if (values[2] < -90.0f) {
values[2] = -180.0f - values[2];
}
}
/*
* Ok, simulate a track-ball. Project the points onto the virtual
* trackball, then figure out the axis of rotation, which is the cross
* product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
* Note: This is a deformed trackball-- is a trackball in the center,
* but is deformed into a hyperbolic sheet of rotation away from the
* center. This particular function was chosen after trying out
* several variations.
*
* It is assumed that the arguments to this routine are in the range
* (-1.0 ... 1.0)
*/
void
trackball(float q[4], float p1x, float p1y, float p2x, float p2y)
{
float a[3]; /* Axis of rotation */
float phi; /* how much to rotate about axis */
float p1[3], p2[3], d[3];
float t;
if (p1x == p2x && p1y == p2y) {
/* Zero rotation */
vzero(q);
q[3] = 1.0;
return;
}
/*
* First, figure out z-coordinates for projection of P1 and P2 to
* deformed sphere
*/
vset(p1,p1x,p1y,tb_project_to_sphere(TRACKBALLSIZE,p1x,p1y));
vset(p2,p2x,p2y,tb_project_to_sphere(TRACKBALLSIZE,p2x,p2y));
/*
* Now, we want the cross product of P1 and P2
*/
vcross(p2,p1,a);
/*
* Figure out how much to rotate around that axis.
*/
vsub(p1,p2,d);
t = vlength(d) / (2.0f*TRACKBALLSIZE);
/*
* Avoid problems with out-of-control values...
*/
if (t > 1.0) t = 1.0;
if (t < -1.0) t = -1.0;
phi = 2.0f * asinf(t);
axis_to_quat(a,phi,q);
}
void Matrix4::toHeadPitchRoll(float &headDegrees, float &pitchDegrees, float &rollDegrees) const{
// Extracts the Euler angles from a rotation matrix. The returned
// angles are in degrees. This method might suffer from numerical
// imprecision for ill defined rotation matrices.
//
// This function only works for rotation matrices constructed using
// the popular NASA standard airplane convention of heading-pitch-roll
// (i.e., RzRxRy).
//
// The algorithm I use here is from a paper written by David Eberly
// titled "Euler Angle Formulas". This paper can be found on his
// Magic Software web site (http://www.magic-software.com).
float thetaX = asinf(mtx[1][2]);
float thetaY = 0.0f;
float thetaZ = 0.0f;
if (thetaX < Math::HALF_PI)
{
if (thetaX > -Math::HALF_PI)
{
thetaZ = atan2f(-mtx[1][0], mtx[1][1]);
thetaY = atan2f(-mtx[0][2], mtx[2][2]);
}
else
{
// Not a unique solution.
thetaZ = -atan2f(mtx[2][0], mtx[0][0]);
thetaY = 0.0f;
}
}
else
{
// Not a unique solution.
thetaZ = atan2f(mtx[2][0], mtx[0][0]);
thetaY = 0.0f;
}
headDegrees = Math::radiansToDegrees(thetaY);
pitchDegrees = Math::radiansToDegrees(thetaX);
rollDegrees = Math::radiansToDegrees(thetaZ);
}
// ****** find roll, pitch, yaw from quaternion ********
void CoordinateConversions::Quaternion2RPY(const float q[4], float rpy[3])
{
float R13, R11, R12, R23, R33;
float q0s = q[0] * q[0];
float q1s = q[1] * q[1];
float q2s = q[2] * q[2];
float q3s = q[3] * q[3];
R13 = 2 * (q[1] * q[3] - q[0] * q[2]);
R11 = q0s + q1s - q2s - q3s;
R12 = 2 * (q[1] * q[2] + q[0] * q[3]);
R23 = 2 * (q[2] * q[3] + q[0] * q[1]);
R33 = q0s - q1s - q2s + q3s;
rpy[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2
rpy[2] = RAD2DEG * atan2f(R12, R11);
rpy[0] = RAD2DEG * atan2f(R23, R33);
//TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2
}
csVector3 csQuaternion::GetEulerAngles () const
{
csVector3 angles;
const float case1 = HALF_PI;
const float case2 = -HALF_PI;
angles.z = atan2f (2.0f * (v.x*v.y + w*v.z), (w*w + v.x*v.x - v.y*v.y - v.z*v.z));
float sine = -2.0f * (v.x*v.z - w*v.y);
if (sine >= 1) // cases where value is 1 or -1 cause NAN
angles.y = case1;
else if (sine <= -1)
angles.y = case2;
else
angles.y = asinf (sine);
angles.x = atan2f (2.0f * (w*v.x + v.y*v.z), (w*w - v.x*v.x - v.y*v.y + v.z*v.z)) ;
return angles;
}
EulerAngles::EulerAngles(const Dcm &dcm) :
Vector(3)
{
setTheta(asinf(-dcm(2, 0)));
if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) {
setPhi(0.0f);
setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1)) + getPhi());
} else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) {
setPhi(0.0f);
setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1)) - getPhi());
} else {
setPhi(atan2f(dcm(2, 1), dcm(2, 2)));
setPsi(atan2f(dcm(1, 0), dcm(0, 0)));
}
}
void Quaternion::ToEuler(float& oRoll, float& oPitch, float& oYaw) {
float a = 2.0f * (s * vy - vx * vz);
if (a < 1.0f) {
if (-1.0f < a) {
oRoll = atan2f(2.0f * (vy * vz + s * vx), 1.0f - 2.0f * (vx * vx + vy * vy));
oPitch = asinf(a);
oYaw = atan2f(2.0f * (vx * vy + s * vz), 1.0f - 2.0f * (vy * vy + vz * vz));
}
else {
oRoll = -atan2f(2.0f * (vx * vy - s * vz), 1.0f - 2.0f * (vx * vx + vz * vz));
oPitch = Pi / -2.0f;
oYaw = 0.0f;
}
}
else {
oRoll = atan2f(2.0f * (vx * vy - s * vz), 1.0f - 2.0f * (vx * vx + vz * vz));
oPitch = Pi / 2.0f;
oYaw = 0.0f;
}
}
void Camera::setOrientation(const Quaternion &newOrientation)
{
Matrix4 m = newOrientation.toMatrix4();
// Store the pitch for this new orientation.
// First person and spectator behaviors limit pitching to
// 90 degrees straight up and down.
m_accumPitchDegrees = Math::radiansToDegrees(asinf(m[1][2]));
// First person and spectator behaviors don't allow rolling.
// Negate any rolling that might be encoded in the new orientation.
m_orientation = newOrientation;
if (m_behavior == CAMERA_BEHAVIOR_FIRST_PERSON || m_behavior == CAMERA_BEHAVIOR_SPECTATOR)
lookAt(m_eye, m_eye + m_viewDir, WORLD_YAXIS);
updateViewMatrix();
}
void Enemy::Update(float fDeltaTime)
{
sf::Uint8 healthVal = (2.55f * m_fHealth);
setColor(sf::Color(getColor().r, healthVal, healthVal, 255));
sf::Vector2f dir(sf::Vector2f(g_player->getPosition().x,g_player->getPosition().y) - getPosition());
dir = MathHelper::Normalize(dir);
float newRot = asinf(dir.x) * 180 / PI;
if(dir.y >0)
{
newRot = 180 - newRot;
}
setRotation(newRot);
m_fShootTimer += fDeltaTime;
if(MathHelper::Distance(g_player->getPosition(), getPosition()) > 350)
{
move(MathHelper::Normalize(g_player->getPosition() - getPosition()) * m_fMaximumSpeed * fDeltaTime);
}
else
{
if(MathHelper::Distance(g_player->getPosition(), getPosition()) > 100 && MathHelper::Distance(g_player->getPosition(), getPosition()) < 350)
{
if(m_fShootTimer > fSHOOT_TIME)
{
m_fShootTimer = 0;
Shoot();
}
}
else
{
move(MathHelper::Normalize(getPosition() - g_player->getPosition()) * m_fMaximumSpeed * 0.5f * fDeltaTime);
}
}
if(m_fHealth < 1)
{
m_bIsDead = true;
}
}
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
{
const float qx2 = q->qx * q->qx;
const float qy2 = q->qy * q->qy;
const float qz2 = q->qz * q->qz;
const float qiqx = q->qi * q->qx;
const float qiqy = q->qi * q->qy;
const float qiqz = q->qi * q->qz;
const float qxqy = q->qx * q->qy;
const float qxqz = q->qx * q->qz;
const float qyqz = q->qy * q->qz;
const float dcm00 = 1.0 - 2.*(qy2 + qz2);
const float dcm01 = 2.*(qxqy + qiqz);
const float dcm02 = 2.*(qxqz - qiqy);
const float dcm12 = 2.*(qyqz + qiqx);
const float dcm22 = 1.0 - 2.*(qx2 + qy2);
e->phi = atan2f(dcm12, dcm22);
e->theta = -asinf(dcm02);
e->psi = atan2f(dcm01, dcm00);
}
void navUkfMatrixExtractEuler(float *m, float *yaw, float *pitch, float *roll)
{
if (m[1 * 3 + 0] > 0.998f)
{ // singularity at north pole
*pitch = atan2f(m[0 * 3 + 2], m[2 * 3 + 2]);
*yaw = MATH_PI / 2.0f;
*roll = 0.0f;
}
else if (m[1 * 3 + 0] < -0.998f)
{ // singularity at south pole
*pitch = atan2f(m[0 * 3 + 2], m[2 * 3 + 2]);
*yaw = -MATH_PI / 2.0f;
*roll = 0.0f;
}
else
{
*pitch = atan2f(-m[2 * 3 + 0], m[0 * 3 + 0]);
*yaw = asinf(m[1 * 3 + 0]);
*roll = atan2f(-m[1 * 3 + 2], m[1 * 3 + 1]);
}
}