Building Quaternion from Scratch, understanding RigidBody Orientation

3dpoints
private Vector4 QuaternionAngle(Vector3 a, Vector3 b,Vector3 c){
a=Normalize(a);
b=Normalize(b);
c=Normalize(c);
Vector3 vNormal = cross(a-b,b-c);
vNormal=Normalize(vNormal);
float fAng = DirectionAngle(a,b,c);
Vector4 Quaternion= Vector3.zero;
float hAng=0.5f*fAng;
Quaternion.x = (float)Math.Sin(hAng)*vNormal.x;
Quaternion.y = (float)Math.Sin(hAng)*vNormal.y;
Quaternion.z = (float)Math.Sin(hAng)*vNormal.z;
Quaternion.w = (float)Math.Cos(hAng);
return Quaternion;
}

private float DirectionAngle(Vector3 _a, Vector3 _b, Vector3 _c)
{

Vector3 a =(_a - _b);
Vector3 b =(_b - _c);
float alpha = (float)Math.Acos((a.x*b.x+a.y*b.y+a.z*b.z)/(Vector3Size(a)*Vector3Size(b)));
return alpha; //return value in radian

}

private Vector3 cross(Vector3 a, Vector3 b)
{

Vector3 c;
c.x=a.y *b.z - a.z *b.y;
c.y=a.z *b.x - a.x *b.z;
c.z=a.x *b.y - a.y *b.x;

return c;
}
private float Vector3Size(Vector3 a)
{
return (float)Math.Sqrt(a.x*a.x+a.y*a.y+a.z*a.z);
}
private Vector3 Normalize(Vector3 a)
{
    Vector3 Result;
    float size = Vector3Size(a);
    Result.x=a.x/size;
    Result.y=a.y/size;
    Result.z=a.z/size;
    return Result;
}

how to use

Vector4 Quat=QuaternionAngle(pointa,pointb,pointc);
// Vector4 is float structure with 4 variables
struct Vector4{
float x;
float y;
float z;
float w;
}

// Vector3 is float structure with 3 variables
struct Vector3
{
float x;
float y;
float z;
}
Advertisements

One thought on “Building Quaternion from Scratch, understanding RigidBody Orientation

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s