void update (quat& q_cur, quat& q_new, bool interpolate)
{
if (interpolate) {
// DO INTERPOLATION
quat q_diff = q_cur.inverse() * q_new; // q_cur * q_diff = q_new
q_diff.normalize();
const float limit_angle = 20.f;
const float threshold = limit_angle*.5f*ZZ_TO_RAD; // threshold = (limit_angle/2)
const float adjust_angle = 10.0f;
const float adjust_threshold = adjust_angle*.5f*ZZ_TO_RAD;
// we assume that q.w = cos(theta/2).
// CAUTION: theta may be [-pi, pi], but acosf assumes that theta is [0, 2pi]
float theta_d2 = acosf(q_diff.w); // theta >= 0
// if got the big angle(> 180 degree)
if (theta_d2 > threshold) {
// construct vector componet
vec3 v(q_diff.x, q_diff.y, q_diff.z);
if (theta_d2 > ZZ_PI*.5f) { // do opposite direction, if exceeds pi/2
v = -v; // invert direction
}
axis_to_quat(q_diff, v, adjust_threshold);
q_cur = q_cur * q_diff;
q_cur.normalize();
}
else {
q_cur = q_new;
}
}
else {
q_cur = q_new;
}
}
void ErrorStateKF<Scalar>::predict(const ErrorStateKF<Scalar>::vec3 &wbody,
const ErrorStateKF<Scalar>::mat3 &varW,
const ErrorStateKF<Scalar>::vec3 &abody,
const ErrorStateKF<Scalar>::mat3 &varA,
Scalar dt) {
const vec3 ah = abody - ba_; // corrected inputs
const vec3 wh = wbody - bg_;
const mat3 wRb = q_.matrix(); // current orientation
// integrate state forward w/ euler equations
const quat dq =
q_ * quat(0, wh[0] * 0.5, wh[1] * 0.5, wh[2] * 0.5);
q_.w() += dq.w() * dt;
q_.x() += dq.x() * dt;
q_.y() += dq.y() * dt;
q_.z() += dq.z() * dt;
q_.normalize();
p_ += v_ * dt;
v_ += (wRb * ah + vec3(0, 0, -g_)) * dt;
// construct error-state jacobian
mat15 F;
F.setZero();
/// dth = [wh] x dth - bg
F.template block<3, 3>(0, 0) = -kr::skewSymmetric(wh);
F.template block<3, 3>(0, 3) = -mat3::Identity();
// dv = -R[ah] x dth - R[ba]
F.template block<3, 3>(6, 0) = -wRb * kr::skewSymmetric(ah);
F.template block<3, 3>(6, 9) = -wRb;
// dp = dv
F.template block<3, 3>(12, 6).setIdentity();
// form process covariance matrix
Eigen::Matrix<Scalar, 12, 12> Q;
Q.setZero();
Q.template block<3, 3>(0, 0) = varW;
Q.template block<3, 3>(3, 3) = Qbg_;
Q.template block<3, 3>(6, 6) = varA;
Q.template block<3, 3>(9, 9) = Qba_;
Eigen::Matrix<Scalar,15,12> G;
G.setZero();
// angular vel. variance on error state angle
G.template block<3, 3>(0, 0) = mat3::Identity() * -1;
G.template block<3, 3>(3, 3).setIdentity();
G.template block<3, 3>(6, 6) = -wRb; // acceleration on linear velocity
G.template block<3, 3>(9, 9).setIdentity();
// integrate covariance forward
P_ += (F * P_ + P_ * F.transpose() + G * Q * G.transpose()) * dt;
}
quat quat::squad(const quat& q1, const quat& q2, const quat& q3, float t, bool shortestPath) const
{
float slerpT(2.0f * t * (1.0f - t));
quat slerpP(slerp(q3, t, shortestPath));
quat slerpQ(q1.slerp(q2, t, false));
return slerpP.slerp(slerpQ, slerpT, shortestPath);
}
static void renderCapsule(Scene *scene, AbstractRender *r, float diameter, float height, const vec3 &pos, const quat &rot, float wire = false) {
mat4 matrix;
if (height < diameter) height = diameter;
float capsule_cylinder_height = height - diameter;
if (capsule_cylinder_height > EPSILON) {
matrix = mat4::scale(vec3(diameter, diameter, capsule_cylinder_height)) * rot.getMatrix() * mat4::translate(pos);
r->setMatrix(matrix);
r->drawMesh(scene->getPrimitive(6), wire);
}
matrix = mat4::scale(vec3(diameter, diameter, diameter)) * mat4::translate(vec3(0.0f, 0.0f, capsule_cylinder_height * 0.5f)) * rot.getMatrix() * mat4::translate(pos);
r->setMatrix(matrix);
r->drawMesh(scene->getPrimitive(4), wire);
matrix = mat4::scale(vec3(diameter, diameter, diameter)) * mat4::rotateX(PI) * mat4::translate(vec3(0.0f, 0.0f, -capsule_cylinder_height * 0.5f)) * rot.getMatrix() * mat4::translate(pos);
r->setMatrix(matrix);
r->drawMesh(scene->getPrimitive(4), wire);
}
void display(){
//描画
glClearColor(0,0,0,0);
//glClearColor(0.2,0.2,0.2,1.0);
//glClearColor(1.0,1.0,1.0,1.0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
gluLookAt(m_world_eye_pos.x, m_world_eye_pos.y, m_zoom,0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
GLfloat ambient[]={76.0/255.0,78.0/255.0,213.0/255.0,1.0};//{0.2,0.2,0.2,1.0};
GLfloat position[]={0.5,0.5,2.4,1.0};
GLfloat mat_diffuse[]={106.0/255.0,121.0/255.0,1.0};//{0.6,0.8,0.0,1.0};
GLfloat mat_specular[]={75.0/255.0,77.0/255.0,231.0/255.0,1.0};//{1.0,1.0,1.0,1.0};
GLfloat mat_shiness[]={5.0};
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
//glLightfv(GL_LIGHT0,GL_AMBIENT,ambient);
glLightfv(GL_LIGHT0,GL_POSITION,position);
glMaterialfv(GL_FRONT,GL_DIFFUSE,mat_diffuse);
glMaterialfv(GL_FRONT,GL_SPECULAR,mat_specular);
glMaterialfv(GL_FRONT,GL_SHININESS,mat_shiness);
glTranslated(m_translate.x,m_translate.y,0.0);/* 平行移動(奥行き方向) */
m_target_quaternion.toMat4(const_cast<float*>(m_rotation_matrix.m));
glMultMatrixf(m_rotation_matrix.m);//クォータニオンによる回転
glEnable(GL_LINE_SMOOTH);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
glHint(GL_LINE_SMOOTH_HINT,GL_DONT_CARE);
//glLineWidth(2.0);
//glColor4ub(0,255,255,255);
//glColor4ub(153,204,0,200);
//glColor4f(0.0,0.0,0.0,1.0);
//glutWireSphere(1.0,20,20);
glUseProgram(m_glsl);
glutSolidTeapot(1.0);
// glutWireCube(1.0);
//glUseProgram(0);
glDisable(GL_BLEND);
glDisable(GL_LINE_SMOOTH);
glPopMatrix();
}//for display
void Mesh::rotate(const quat q) {
const auto rot = glm::rotate(q.theta(), q.axis());
for (auto& vert : _data.verts) vert = (vec3)(rot * vec4(vert, 0));
for (auto& normal : _data.normals) normal = (vec3)(rot * vec4(normal, 0));
renderData = shared<RenderData>(nullptr);
}
float4x4 MatrixTRS(const float3 & translate, const quat & rotate, const float3 & scale)
{
return Translate(translate) * rotate.ToFloat4x4() * Scale(scale);
}
quat rotate_quat( quat const& v, quat const& q )
{
return q * v * q.inverse();
}
vector3 rotate_vector( vector3 const& v, quat const& q )
{
quat p = quat(v.x(), v.y(), v.z(), 0);
quat tp = q * p * q.inverse();
return vector3(tp.qx(), tp.qy(), tp.qz());
}
void mat4::set_rot(const quat& q)
{
mat3 m;
q.ToMatrix(m);
set_rot(m);
}
mat4 mat4::rotation(const quat &val)
{
return rotation(val.axis_angle());
}
value_t logdist (const quat<value_t>& other) const {
const auto& difference = ((*this)*(other.C())).log();
return difference.dot(difference);
}
m4x4::m4x4(const quat &q){
q.toMatrix(*this);
}
