/**
* Note that the derivative is not generally a unit quaternion.
* \param q the current orientation
* \param w the angular velocity (in the global frame)
* Uses the matrix:
* | -q.x +q.w -q.z +q.y |
* G = | -q.y +q.z +q.w -q.x |
* | -q.z -q.y +q.x +q.w |
*/
QUAT QUAT::deriv(const QUAT& q, const VECTOR3& w)
{
QUAT qd;
qd.w = .5 * (-q.x * w.x() - q.y * w.y() - q.z * w.z());
qd.x = .5 * (+q.w * w.x() + q.z * w.y() - q.y * w.z());
qd.y = .5 * (-q.z * w.x() + q.w * w.y() + q.x * w.z());
qd.z = .5 * (+q.y * w.x() - q.x * w.y() + q.w * w.z());
return qd;
}
/**
* This matrix is used in the relationships qd = 1/2*L^T*omega and
* qdd = 1/2*L^T*alpha' - 1/4*omega'^2*q, where omega'/alpha' are the angular
* velocity/acceleration of a rigid body in the body frame and qd/qdd are the
* first/second time derivatives of the Euler (unit quaternion) parameters.
*
* The matrix L is defined as:
* -e1 e0 e3 -e2
* -e2 -e3 e0 e1
* -e3 e2 -e1 e0
*/
QUAT QUAT::L_transpose_mult(const VECTOR3& v) const
{
double de0 = -x*v.x() - y*v.y() - z*v.z();
double de1 = +w*v.x() - z*v.y() + y*v.z();
double de2 = +z*v.x() + w*v.y() - x*v.z();
double de3 = -y*v.x() + x*v.y() + w*v.z();
QUAT q;
q.w = de0;
q.x = de1;
q.y = de2;
q.z = de3;
return q;
}
/// Computes the angular velocity of a body given the current quaternion orientation and the quaternion velocity
VECTOR3 QUAT::to_omega(const QUAT& q, const QUAT& qd)
{
VECTOR3 omega;
omega.x() = 2 * (-q.x * qd.w + q.w * qd.x - q.z * qd.y + q.y * qd.z);
omega.y() = 2 * (-q.y * qd.w + q.z * qd.x + q.w * qd.y - q.x * qd.z);
omega.z() = 2 * (-q.z * qd.w - q.y * qd.x + q.x * qd.y + q.w * qd.z);
return omega;
}
/**
* This matrix is used in the relationships omega = 2*G*qd and
* alpha = 2*G*qdd, where omega/alpha are the angular velocity/acceleration
* of a rigid body in the game frame and qd/qdd are the first/second time
* derivatives of the Euler (unit quaternion) parameters.
*/
VECTOR3 QUAT::G_mult(REAL qx, REAL qy, REAL qz, REAL qw) const
{
const double e0 = qw;
const double e1 = qx;
const double e2 = qy;
const double e3 = qz;
VECTOR3 r;
r.x() = -x*e0 + w*e1 - z*e2 + y*e3;
r.y() = -y*e0 + z*e1 + w*e2 - x*e3;
r.z() = -z*e0 - y*e1 + x*e2 + w*e3;
return r;
}
/**
* This matrix is used in the relationships omega' = 2*L*qd and
* alpha' = 2*L*qdd, where omega'/alpha' are the angular velocity/acceleration
* of a rigid body in the body's frame and qd/qdd are the first/second time
* derivatives of the Euler (unit quaternion) parameters.
*
* The matrix L is defined as:
* -e1 e0 e3 -e2
* -e2 -e3 e0 e1
* -e3 e2 -e1 e0
*/
VECTOR3 QUAT::L_mult(REAL qx, REAL qy, REAL qz, REAL qw) const
{
const double& e0 = w;
const double& e1 = x;
const double& e2 = y;
const double& e3 = z;
VECTOR3 v;
v.x() = -e1*qw + e0*qx + e3*qy - e2*qz;
v.y() = -e2*qw - e3*qx + e0*qy + e1*qz;
v.z() = -e3*qw + e2*qx - e1*qy + e0*qz;
return v;
}
void get_color_entropy(float& r,float& g,float& b,float& a,VECTOR3 p,int* grid_res)
{
if(!entropies)
return;
int x=p.x(); int y=p.y(); int z=p.z();
int idx=x+y*grid_res[0]+z*grid_res[0]*grid_res[1];
float val=entropies[idx];
r=val;
if(val<0.5)
g=2*val;
else
g=2-2*val;
b=1-val;
a=val;
}
//load seeds
float* get_grid_vec_data(int* grid_res)//get vec data at each grid point
{
osuflow->GetFlowField()->getDimension(grid_res[0],grid_res[1],grid_res[2]);
float * vectors=new float[grid_res[0]*grid_res[1]*grid_res[2]*3];
for(int k=0; k<grid_res[2];k++)
{
for(int j=0; j<grid_res[1];j++)
{
for(int i=0; i<grid_res[0];i++)
{
VECTOR3 data;
osuflow->GetFlowField()->at_vert(i,j,k,0,data);//t=0, static data
int idx=i+j*grid_res[0]+k*grid_res[0]*grid_res[1];
data.Normalize();
vectors[idx*3+0]=data.x();
vectors[idx*3+1]=data.y();
vectors[idx*3+2]=data.z();
}
}
}
return vectors;
}