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); } }
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); } }
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]; } }
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); } }
/*-------------------------------------------------------------------------*/ 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; }
void FromAngleAxisPt(Quaternion* q, float rdAngle, Point3* rkPoint) { FromAngleAxis(q, rdAngle, rkPoint->x, rkPoint->y, rkPoint->z); }
cQuaternion::cQuaternion(float afAngle, const cVector3f & avAxis) { FromAngleAxis(afAngle,avAxis); }
void CQuater::FromAngleAxis(float fAngle, const CVertex& v) { FromAngleAxis(fAngle, v.x, v.y, v.z); }
Quat::Quat(Real radian, const Vec3& axis) { FromAngleAxis(radian, axis); }