Пример #1
0
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)));
}
Пример #2
0
/*-----------------------------------------------------------------------------*\
 * 从四元数转换为欧拉角表示 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 );
	}
}
Пример #3
0
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
  }
Пример #4
0
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);
}
Пример #5
0
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
}
Пример #6
0
/* 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;
}
Пример #7
0
// 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;
}
Пример #8
0
/**
 * 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);
    }
}
Пример #9
0
// 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;
}
Пример #10
0
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;
	}
}
Пример #11
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;
}
Пример #12
0
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);
}
Пример #13
0
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;
}
Пример #14
0
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));
}
Пример #15
0
	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();
	}
Пример #16
0
  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 ();
      }
    }
  }
Пример #17
0
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;
}
Пример #18
0
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);
}
Пример #19
0
//-----------------------------------------------------------------------------
// 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];
    }
}
Пример #21
0
/*
 * 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);
}
Пример #22
0
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
}
Пример #24
0
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;
}
Пример #25
0
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)));
	}
}
Пример #26
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();
}
Пример #28
0
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;
	}
}
Пример #29
0
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);
}
Пример #30
0
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]);
	}
}