Exemplo n.º 1
0
void Quaternion::FromRotationTo(const Vector3& start, const Vector3& end)
{
    Vector3 normStart = start.Normalized();
    Vector3 normEnd = end.Normalized();
    float d = normStart.DotProduct(normEnd);
    
    if (d > -1.0f + M_EPSILON)
    {
        Vector3 c = normStart.CrossProduct(normEnd);
        float s = sqrtf((1.0f + d) * 2.0f);
        float invS = 1.0f / s;
        
        x_ = c.x_ * invS;
        y_ = c.y_ * invS;
        z_ = c.z_ * invS;
        w_ = 0.5f * s;
    }
    else
    {
        Vector3 axis = Vector3::RIGHT.CrossProduct(normStart);
        if (axis.Length() < M_EPSILON)
            axis = Vector3::UP.CrossProduct(normStart);
        
        FromAngleAxis(180.f, axis);
    }
}
Exemplo n.º 2
0
void Quat::FromDirection(const Vec3& dir){
	Real angle = Vec3::UNIT_Y.AngleBetween(dir);
	if (IsEqual(angle, 0.f, 0.001f)){
		*this = IDENTITY;
	}
	else{
		auto axis = Vec3::UNIT_Y.Cross(dir).NormalizeCopy();
		FromAngleAxis(angle, axis);
	}
}
Exemplo n.º 3
0
void _GetMatrix(Animation* animation, double time, double m[4][4])
{
   Animation* a = (Animation*) animation;
   
   /* m = translation * rotation * scale */
   
   /* Rotation. */
   if(a->rotations)
   {
      double drot[4];
      _GetRotation(a, time, drot);
      
      Rotation3 RR;
      RR.x = drot[0];
      RR.y = drot[1];
      RR.z = drot[2];
      RR.angle = drot[3];
      
      Quaternion q;
      FromAngleAxis(&q, RR.angle, RR.x, RR.y, RR.z);
      QuatToMatrix(&q, m); /* Also sets bottom row to 0 0 0 1 */
   }
   else
   {
      m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
      m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0;
      m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = 0;
      m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
   }

   /* Concatenate with scaling. */
   if(a->scales)
   {
      double S[3];
      _GetScale(a, time, S);

      m[0][0] *= S[0]; m[0][1] *= S[1]; m[0][2] *= S[2];
      m[1][0] *= S[0]; m[1][1] *= S[1]; m[1][2] *= S[2];
      m[2][0] *= S[0]; m[2][1] *= S[1]; m[2][2] *= S[2];
   }

   /* Translation. */
   if(a->translations)  
   {
      double P[3];
      _GetTranslation(a, time, P);
      
      m[0][3] = P[0];
      m[1][3] = P[1];
      m[2][3] = P[2];
   }
}
Exemplo n.º 4
0
void Quaternion::FromRotationTo(const Vector3& start, const Vector3& end)
{
	Vector3 normStart = start.Normalized();
	Vector3 normEnd = end.Normalized();
	float d = normStart.Dot(normEnd);

	if (d > -1.0f + Math::kLARGE_EPSILON) {
		Vector3 c = normStart.Cross(normEnd);
		float s = sqrtf((1.0f + d) * 2.0f);
		float invS = 1.0f / s;

		x = c.x * invS;
		y = c.y * invS;
		z = c.z * invS;
		w = 0.5f * s;
	}
	else {
		Vector3 axis = Vector3::RIGHT.Cross(normStart);
		if (axis.Length() < Math::kLARGE_EPSILON)
			axis = Vector3::UP.Cross(normStart);

		FromAngleAxis(180.f, axis);
	}
}
Exemplo n.º 5
0
/*-------------------------------------------------------------------------*/
void* KB_RotInitialize (int numKeys, RotationKey* key)
{
    double omt0, omc0, opc0, omb0, opb0, adj0, out0, out1;
    double omt1, omc1, opc1, omb1, opb1, adj1, in0, in1;
    int i0 = 0, i1 = 1, i2 = 2, i3 = 3;
    Quaternion q0, q1, q2, q3;
    Quaternion prod;
    Quaternion logdq, Tout, Tin, arg0, arg1;

    /* assert:  numKeys >= 4 */

    SplineInfo* info = (SplineInfo*) calloc( 1, sizeof(SplineInfo));
    info->numPolys = numKeys-3;
    info->si = (SquadInfo*) calloc(info->numPolys, sizeof( SquadInfo ) );

    for (/**/; i0 < info->numPolys; i0++, i1++, i2++, i3++)
    {
        FromAngleAxis(&q0, key[i0].Rot.angle,key[i0].Rot.x,key[i0].Rot.y,
            key[i0].Rot.z);
        FromAngleAxis(&q1, key[i1].Rot.angle,key[i1].Rot.x,key[i1].Rot.y,
            key[i1].Rot.z);
        FromAngleAxis(&q2, key[i2].Rot.angle,key[i2].Rot.x,key[i2].Rot.y,
            key[i2].Rot.z);
        FromAngleAxis(&q3, key[i3].Rot.angle,key[i3].Rot.x,key[i3].Rot.y,
            key[i3].Rot.z);

        /* arrange for consecutive quaternions to have acute angle */
        if ( Dot(&q1, &q2) < 0.0 )
        {
	    /*q2 = -q2;*/
	    NegSelf(&q2);
            ToAngleAxis(&q2, &(key[i2].Rot.angle),&(key[i2].Rot.x),&(key[i2].Rot.y),
                &(key[i2].Rot.z));
        }

        /* build log(q[i1]^{-1}*q[i2]) */
	/* prod = q1.UnitInverse()*q2; */
	UnitInverse(&prod, &q1); MulSelf(&prod, &q2);
        /* logdq = prod.Log(); */
	Log(&logdq, &prod);

	/* build multipliers at q[i1] */
        omt0 = 1-key[i1].tension;
        omc0 = 1-key[i1].continuity;
        opc0 = 1+key[i1].continuity;
        omb0 = 1-key[i1].bias;
	opb0 = 1+key[i1].bias;
	adj0 = 2*(key[i2].t-key[i1].t)/(key[i2].t-key[i0].t);
	out0 = 0.5*adj0*omt0*opc0*opb0;
	out1 = 0.5*adj0*omt0*omc0*omb0;

        /* build outgoing tangent at q[i1] */
        /* prod = q0.UnitInverse()*q1; */
	UnitInverse(&prod, &q0); MulSelf(&prod, &q1);
        /* Quaternion Tout = out1*logdq+out0*prod.Log(); */
	MulScal(&Tout, &logdq, out1); LogSelf(&prod); MulScal(&prod, &prod, out0);
	AddSelf(&Tout, &prod);

        /* build multipliers at q[i2] */
	omt1 = 1-key[i2].tension;
	omc1 = 1-key[i2].continuity;
	opc1 = 1+key[i2].continuity;
	omb1 = 1-key[i2].bias;
	opb1 = 1+key[i2].bias;
	adj1 = 2*(key[i2].t-key[i1].t)/(key[i3].t-key[i1].t);
	in0 = 0.5*adj1*omt1*omc1*opb1;
	in1 = 0.5*adj1*omt1*opc1*omb1;

        /* build incoming tangent at q[i2] */
        /* prod = q2.UnitInverse()*q3; */
	UnitInverse(&prod, &q2); MulSelf(&prod, &q3);
	/* Quaternion Tin = in1*prod.Log()+in0*logdq; */
	MulScal(&Tin, &logdq, in0); LogSelf(&prod); MulScal(&prod, &prod, in1);
	AddSelf(&Tin, &prod);

        /*info->si[i0].p = q1;*/
	SetQuat(&(info->si[i0].p), &q1);
	  
        /*info->si[i0].q = q2; */
	SetQuat(&(info->si[i0].q), &q2);


        /* Quaternion arg0 = 0.5*(Tout-logdq); */
	Sub(&arg0, &Tout, &logdq); MulScalSelf(&arg0, 0.5);
	/* Quaternion arg1 = 0.5*(logdq-Tin); */
	Sub(&arg1, &logdq, &Tin); MulScalSelf(&arg1, 0.5);

        /* info->si[i0].a = q1*arg0.Exp(); */	
        /* info->si[i0].b = q2*arg1.Exp(); */
	ExpSelf(&arg0); Mul(&(info->si[i0].a), &q1, &arg0);
	ExpSelf(&arg1); Mul(&(info->si[i0].b), &q2, &arg1);

        info->si[i0].tmin = key[i1].t;
        info->si[i0].tmax = key[i2].t;
        info->si[i0].trange = info->si[i0].tmax-info->si[i0].tmin;
    }

    return info;
}
Exemplo n.º 6
0
void FromAngleAxisPt(Quaternion* q, float rdAngle, Point3* rkPoint)
{
    FromAngleAxis(q, rdAngle, rkPoint->x, rkPoint->y, rkPoint->z);
}
Exemplo n.º 7
0
cQuaternion::cQuaternion(float afAngle, const cVector3f & avAxis)
{
    FromAngleAxis(afAngle,avAxis);
}
Exemplo n.º 8
0
void CQuater::FromAngleAxis(float fAngle, const CVertex& v) {
	FromAngleAxis(fAngle, v.x, v.y, v.z);
}
Exemplo n.º 9
0
Quat::Quat(Real radian, const Vec3& axis)
{
	FromAngleAxis(radian, axis);
}