/* apply_quat:
* Multiplies the point (x, y, z) by the quaternion q, storing the result in
* (*xout, *yout, *zout). This is quite a bit slower than apply_matrix_f.
* So, only use it if you only need to translate a handful of points.
* Otherwise it is much more efficient to call quat_to_matrix then use
* apply_matrix_f.
*/
void apply_quat(AL_CONST QUAT *q, float x, float y, float z, float *xout, float *yout, float *zout)
{
QUAT v;
QUAT i;
QUAT t;
ASSERT(q);
ASSERT(xout);
ASSERT(yout);
ASSERT(zout);
/* v' = q * v * q^-1 */
v.w = 0;
v.x = x;
v.y = y;
v.z = z;
/* NOTE: Rotating about a zero quaternion is undefined. This can be shown
* by the fact that the inverse of a zero quaternion is undefined
* and therefore causes an exception below.
*/
ASSERT(!((q->x == 0.0) && (q->y == 0.0) && (q->z == 0.0) && (q->w == 0.0)));
quat_inverse(q, &i);
quat_mul(&i, &v, &t);
quat_mul(&t, q, &v);
*xout = v.x;
*yout = v.y;
*zout = v.z;
}
union quat *quat_conjugate(union quat *qo, union quat *rotation, union quat *new_coord_system)
{
union quat temp, inverse;
/* Convert rotation to new coordinate system */
quat_mul(&temp, new_coord_system, rotation);
quat_inverse(&inverse, new_coord_system);
quat_mul(qo, &temp, &inverse);
return qo;
}
/* Apply incremental yaw and pitch relative to the quaternion.
* Yaw is applied to world axis so no roll will accumulate */
union quat *quat_apply_relative_yaw_pitch(union quat *q, double yaw, double pitch)
{
union quat qyaw, qpitch, q1;
/* calculate amount of yaw to impart this iteration... */
quat_init_axis(&qyaw, 0.0, 1.0, 0.0, yaw);
/* Calculate amount of pitch to impart this iteration... */
quat_init_axis(&qpitch, 0.0, 0.0, 1.0, pitch);
quat_mul(&q1, &qyaw, q);
quat_mul(q, &q1, &qpitch);
return q;
}
static int main_da_motion_notify(int x, int y)
{
float cx, cy, lx, ly;
union vec3 v1, v2;
union quat rotation;
if (!isDragging) {
lastcount = 0;
} else {
if (lastcount < MOUSE_HISTORY) {
last++;
lastx[last % MOUSE_HISTORY] =
2.0 * (((float) x / (float) real_screen_width) - 0.5);
lasty[last % MOUSE_HISTORY] =
2.0 * (((float) y / (float) real_screen_height) - 0.5);
lastcount++;
return 0;
}
lastcount++;
lx = lastx[(last + 1) % MOUSE_HISTORY];
ly = lasty[(last + 1) % MOUSE_HISTORY];
last = (last + 1) % MOUSE_HISTORY;
cx = 2.0 * (((float) x / (float) real_screen_width) - 0.5);
cy = 2.0 * (((float) y / (float) real_screen_height) - 0.5);
lastx[last] = cx;
lasty[last] = cy;
v1.v.z = 0;
v1.v.y = 0;
v1.v.x = -1.0;
v2.v.z = cx - lx;
v2.v.y = cy - ly;
v2.v.x = -1.0;
quat_from_u2v(&rotation, &v1, &v2, 0);
if (isDraggingLight) {
quat_mul(&light_orientation, &rotation, &last_light_orientation);
last_light_orientation = light_orientation;
} else {
quat_mul(&lobby_orientation, &rotation, &last_lobby_orientation);
last_lobby_orientation = lobby_orientation;
v2.v.z /= 3.0;
v2.v.y /= 3.0;
quat_from_u2v(&autorotation, &v1, &v2, 0);
autospin_initialized = 1;
}
}
return 0;
}
void do_autospin(void)
{
if (!autospin || !autospin_initialized)
return;
quat_mul(&lobby_orientation, &autorotation, &last_lobby_orientation);
last_lobby_orientation = lobby_orientation;
}
X42_EXPORT bool X42_CALL x42_ApplyBoneOffsets( x42animLerp_t *lerp, const x42data_t *x42,
const x42boneOffset_t *offsets, uint numOffsets, x42opts_t *opts )
{
uint i, j;
REF_PARAM( opts );
#ifndef LIBX42_NO_PARAM_VALIDATION
demand_rf( lerp != NULL, X42_ERR_BADPTR, "lerp is NULL" );
demand_rf( x42 != NULL, X42_ERR_BADPTR, "x42 is NULL" );
demand_rf( x42_ValidateHeader( &x42->header ), X42_ERR_BADDATA, "invalid x42 header data" );
demand_rf( offsets != NULL, X42_ERR_BADPTR, "offsets is NULL" );
#endif
for( i = 0; i < numOffsets; i++ )
{
for( j = 0; j < x42->header.numBones; j++ )
{
if( x42name_eq2( x42, x42->bones[j].name, offsets[i].boneName ) )
break;
}
if( j == x42->header.numBones )
continue;
vec3_add( lerp->p[j], lerp->p[j], offsets[i].pos_offset );
vec3_add( lerp->s[j], lerp->s[j], offsets[i].scale_offset );
quat_mul( lerp->r[j], offsets[i].rot_offset, lerp->r[j] );
}
return true;
}
QuatP* pivot_between_orientation(const struct Pivot* pivot1, const struct Pivot* pivot2, Quat between_rotation) {
Quat inverted_from;
quat_invert(pivot1->orientation, inverted_from);
quat_mul(pivot2->orientation, inverted_from, between_rotation);
return between_rotation;
}
static void simulate_trackball(quat *q, GLfloat p1x, GLfloat p1y, GLfloat p2x, GLfloat p2y) {
if (p1x == p2x && p1y == p2y) {
quat_identity(q);
}
else {
quat p1, p2, a, d;
float p1z, p2z;
float s, t;
p1z = project_to_sphere(p1x, p1y);
quat_assign(&p1, 0.0, p1x, p1y, p1z);
p2z = project_to_sphere(p2x, p2y);
quat_assign(&p2, 0.0, p2x, p2y, p2z);
quat_mul(&a, &p1, &p2);
a.w = 0.0;
s = quat_norm(&a);
quat_div_real(&a, &a, s);
quat_sub(&d, &p1, &p2);
t = quat_norm(&d) / (2.0 * R * ROOT_2_INV);
if (t > 1.0) t = 1.0;
quat_assign(q, cos(asin(t)), a.x * t, a.y * t, a.z * t);
}
}
static void motion(int x, int y) {
static int count = 0;
if (scaling) {
scale_factor = scale_factor * (1.0 + (((float) (begin_y - y)) / height));
begin_x = x;
begin_y = y;
glutPostRedisplay();
return;
}
else {
quat t;
simulate_trackball(&last, (2.0 * begin_x - width) / width, (height - 2.0 * begin_y) / height, (2.0 * x - width) / width, (height - 2.0 * y) / height);
begin_x = x;
begin_y = y;
quat_mul(&t, &last, &curr);
curr = t;
if (++count % 97) {
GLfloat n;
n = quat_norm(&curr);
quat_div_real(&curr, &curr, n);
}
glutPostRedisplay();
}
}
/*
rotate
Internally used helper for CCameraObject::RotateByMouse
*/
static void rotate(float* camPos, float* lookAtPos, float* upVec, float xDir)
{
quat qRot, qView, qNewView;
quat_rotate(qRot, deg_2_rad(xDir), upVec);
qView[0] = lookAtPos[0] - camPos[0];
qView[1] = lookAtPos[1] - camPos[1];
qView[2] = lookAtPos[2] - camPos[2];
qView[3] = 0.0f;
quat_mul(qRot, qView, qNewView);
quat_conjugate(qRot);
quat_mul(qNewView, qRot, qNewView);
lookAtPos[0] = camPos[0] + qNewView[0];
lookAtPos[1] = camPos[1] + qNewView[1];
lookAtPos[2] = camPos[2] + qNewView[2];
}
void quat_decompose_swing_twist(const union quat *q, const union vec3 *v1, union quat *swing, union quat *twist)
{
union vec3 v2;
quat_rot_vec(&v2, v1, q);
quat_from_u2v(swing, v1, &v2, 0);
union quat swing_inverse;
quat_inverse(&swing_inverse, swing);
quat_mul(twist, &swing_inverse, q);
}
void vec_rotate(vector *v, const quat *q)
{
vector tmpv;
quat vecq, q_inv;
/* v = q * v * q' */
tmpv = *v;
vecq.x = tmpv.x;
vecq.y = tmpv.y;
vecq.z = tmpv.z;
vecq.w = 0.0f;
q_inv = *q;
quat_conj(&q_inv, &q_inv);
quat_mul(&vecq, &vecq, &q_inv);
quat_mul(&vecq, q, &vecq);
v->x = vecq.x;
v->y = vecq.y;
v->z = vecq.z;
}
void Camera_offset_orientation(Camera* const camera, float yaw, float pitch) {
quat pitch_rotation, yaw_rotation;
quat_rotate(yaw_rotation, yaw, G_UP);
vec3 right;
quat_mul_vec3(right, camera->transform.orientation, G_RIGHT);
vec3_norm(right, right);
quat_rotate(pitch_rotation, pitch, right);
quat orientation;
quat_mul(orientation, yaw_rotation, pitch_rotation);
quat_mul(
camera->transform.orientation,
orientation,
camera->transform.orientation
);
quat_norm(camera->transform.orientation, camera->transform.orientation);
}
void quat_mul_axis_angle(const Quat q, const Vec3f axis, const float angle, Quat r) {
if( (fabs(axis[0]) < CUTE_EPSILON && fabs(axis[1]) < CUTE_EPSILON && fabs(axis[2]) < CUTE_EPSILON) ||
fabs(angle) < CUTE_EPSILON )
{
quat_copy(q, r);
}
Quat rotation = {0};
quat_from_axis_angle(axis, angle, rotation);
quat_mul(q, rotation, r);
}
struct Pivot* pivot_combine(const struct Pivot* pivot1, const struct Pivot* pivot2, struct Pivot* r) {
Vec4f concat_position = {0};
vec_add(pivot1->position, pivot2->position, concat_position);
Quat concat_orientation = {0};
quat_mul(pivot1->orientation, pivot2->orientation, concat_orientation);
pivot_create(concat_position, concat_orientation, r);
return r;
}
quat_t anm_get_rotation(struct anm_node *node, anm_time_t tm)
{
quat_t rot, prot;
rot = anm_get_node_rotation(node, tm);
if(!node->parent) {
return rot;
}
prot = anm_get_rotation(node->parent, tm);
return quat_mul(prot, rot);
}
void md5_build_joints(Md5_anim *a)
{
int i;
for (i = 0; i < a->num_joints; i++) {
Md5_joint *j = a->joints + i;
Md5_joint *p = j->parent;
if (p != NULL) {
j->pos = v3f_plus(p->pos, quat_rot(p->orient, j->pos));
j->orient = quat_mul(p->orient, j->orient);
}
}
}
quat_t quat_rotate(quat_t q, float angle, float x, float y, float z)
{
quat_t rq;
float half_angle = angle * 0.5;
float sin_half = sin(half_angle);
rq.w = cos(half_angle);
rq.x = x * sin_half;
rq.y = y * sin_half;
rq.z = z * sin_half;
return quat_mul(q, rq);
}
void quat_rot_vec(VEC *q1, VEC *v1){
VEC *quat_of_vec, *qmulv, *qinv, *fin_rot_vec; //all are quaternions
quat_of_vec = v_get(4);
v_zero(quat_of_vec);
qmulv = v_get(4);
v_zero(qmulv);
qinv = v_get(4);
v_zero(qinv);
fin_rot_vec = v_get(4);
v_zero(fin_rot_vec);
vec2quat(v1, quat_of_vec);
quat_inv(q1, qinv);
quat_mul(qinv, quat_of_vec, qmulv);
quat_mul(qmulv, q1, fin_rot_vec);
quat2vec(fin_rot_vec, v1);
v_free(quat_of_vec);
v_free(qmulv);
v_free(qinv);
v_free(fin_rot_vec);
}
/* Apply incremental yaw, pitch and roll relative to the quaternion.
* For example, if the quaternion represents an orientation of a ship,
* this will apply yaw/pitch/roll *in the ship's local coord system to the
* orientation.
*/
union quat *quat_apply_relative_yaw_pitch_roll(union quat *q,
double yaw, double pitch, double roll)
{
union quat qyaw, qpitch, qroll, qrot, tempq, local_rotation, rotated_q;
/* calculate amount of yaw to impart this iteration... */
quat_init_axis(&qyaw, 0.0, 1.0, 0.0, yaw);
/* Calculate amount of pitch to impart this iteration... */
quat_init_axis(&qpitch, 0.0, 0.0, 1.0, pitch);
/* Calculate amount of roll to impart this iteration... */
quat_init_axis(&qroll, 1.0, 0.0, 0.0, roll);
/* Combine pitch, roll and yaw */
quat_mul(&tempq, &qyaw, &qpitch);
quat_mul(&qrot, &tempq, &qroll);
/* Convert rotation to local coordinate system */
quat_conjugate(&local_rotation, &qrot, q);
/* Apply to local orientation */
quat_mul(&rotated_q, &local_rotation, q);
quat_normalize_self(&rotated_q);
*q = rotated_q;
return q;
}
void quat_logdif(AD_Quaternion *q1, AD_Quaternion *q2, AD_Quaternion *q3)
{
AD_Quaternion inv, dif;
float len, len1, s;
quat_inverse (q1, &inv);
quat_mul (&inv, q2, &dif);
len = sqrtf (dif.x*dif.x + dif.y*dif.y + dif.z*dif.z);
s = quat_dot (q1, q2);
if (s != 0.0) len1 = atanf (len / s); else len1 = (float)(M_PI/2.0f);
if (len != 0.0) len1 /= len;
q3->w = 0.0;
q3->x = dif.x * len1;
q3->y = dif.y * len1;
q3->z = dif.z * len1;
}
struct model *load_model(const char *name)
{
char filename[1024];
unsigned char *data = NULL;
struct model *model;
int i, len;
model = lookup(model_cache, name);
if (model)
return model;
for (i = 0; i < nelem(formats); i++) {
strlcpy(filename, name, sizeof filename);
strlcat(filename, formats[i].suffix, sizeof filename);
data = load_file(filename, &len);
if (data)
break;
}
if (!data) {
warn("error: cannot find model: '%s'", name);
return NULL;
}
model = formats[i].load(filename, data, len);
if (!model)
warn("error: cannot load model: '%s'", filename);
free(data);
if (model)
model_cache = insert(model_cache, name, model);
if (model && model->anim) {
struct anim *anim = model->anim;
struct pose a_from_org, b_from_org;
vec4 org_from_a;
extract_frame_root(&a_from_org, anim, 0);
extract_frame_root(&b_from_org, anim, anim->frames - 1);
vec_sub(anim->motion.position, b_from_org.position, a_from_org.position);
quat_conjugate(org_from_a, a_from_org.rotation);
quat_mul(anim->motion.rotation, b_from_org.rotation, org_from_a);
vec_div(anim->motion.scale, b_from_org.scale, a_from_org.scale);
}
return model;
}
static void do_camera_physics(vector *cam_pos, quat *cam_orient)
{
vector move;
quat qturn;
/* Rotation */
camera.turn.x = CAP(turn + mouseturn_x + turn_right-turn_left, 1.0f);
camera.turn.y = CAP(pitch + mouseturn_y + pitch_up-pitch_down, 1.0f);
camera.turn.z = CAP(roll + roll_left-roll_right, 1.0f);
cam_turn_vel.x += camera.turn.x * time_diff * turnaccel;
cam_turn_vel.y += camera.turn.y * time_diff * turnaccel;
cam_turn_vel.z += camera.turn.z * time_diff * turnaccel;
cam_turn_vel.x *= powf(turndrag, -time_diff);
cam_turn_vel.y *= powf(turndrag, -time_diff);
cam_turn_vel.z *= powf(turndrag, -time_diff);
quat_make_euler(&qturn, cam_turn_vel.x*time_diff, cam_turn_vel.y*time_diff*0.75f, cam_turn_vel.z*time_diff*0.75f);
quat_mul(cam_orient, cam_orient, &qturn);
quat_norm(cam_orient, cam_orient);
/* Movement */
camera.move.x = CAP(move_x + slide_right-slide_left, 1.0f);
camera.move.y = CAP(move_y + slide_up-slide_down, 1.0f);
camera.move.z = CAP(move_z + move_forward-move_backward, 1.0f);
move = camera.move;
if (vec_mag(&move))
vec_rotate(&move, cam_orient);
cam_vel.x += move.x * time_diff * accel;
cam_vel.y += move.y * time_diff * accel;
cam_vel.z += move.z * time_diff * accel;
cam_vel.x *= powf(drag, -time_diff);
cam_vel.y *= powf(drag, -time_diff);
cam_vel.z *= powf(drag, -time_diff);
cam_pos->x += cam_vel.x * time_diff;
cam_pos->y += cam_vel.y * time_diff;
cam_pos->z += cam_vel.z * time_diff;
}
void Ukf(VEC *omega, VEC *mag_vec, VEC *mag_vec_I, VEC *sun_vec, VEC *sun_vec_I, VEC *Torq_ext, double t, double h, int eclipse, VEC *state, VEC *st_error, VEC *residual, int *P_flag, double sim_time)
{
static VEC *omega_prev = VNULL, *mag_vec_prev = VNULL, *sun_vec_prev = VNULL, *q_s_c = VNULL, *x_prev = VNULL, *Torq_prev, *x_m_o;
static MAT *Q = {MNULL}, *R = {MNULL}, *Pprev = {MNULL};
static double alpha, kappa, lambda, sqrt_lambda, w_m_0, w_c_0, w_i, beta;
static int n_states, n_sig_pts, n_err_states, iter_num, initialize=0;
VEC *x = VNULL, *x_priori = VNULL, *x_err_priori = VNULL, *single_sig_pt = VNULL, *v_temp = VNULL, *q_err_quat = VNULL,
*err_vec = VNULL, *v_temp2 = VNULL, *x_ang_vel = VNULL, *meas = VNULL, *meas_priori = VNULL,
*v_temp3 = VNULL, *x_posteriori_err = VNULL, *x_b_m = VNULL, *x_b_g = VNULL;
MAT *sqrt_P = {MNULL}, *P = {MNULL}, *P_priori = {MNULL}, *sig_pt = {MNULL}, *sig_vec_mat = {MNULL},
*err_sig_pt_mat = {MNULL}, *result = {MNULL}, *result_larger = {MNULL}, *result1 = {MNULL}, *Meas_err_mat = {MNULL},
*P_zz = {MNULL}, *iP_vv = {MNULL}, *P_xz = {MNULL}, *K = {MNULL}, *result2 = {MNULL}, *result3 = {MNULL}, *C = {MNULL};
int update_mag_vec, update_sun_vec, update_omega, i, j;
double d_res;
if (inertia == MNULL)
{
inertia = m_get(3,3);
m_ident(inertia);
inertia->me[0][0] = 0.007;
inertia->me[1][1] = 0.014;
inertia->me[2][2] = 0.015;
}
if (initialize == 0){
iter_num = 1;
n_states = (7+6);
n_err_states = (6+6);
n_sig_pts = 2*n_err_states+1;
alpha = sqrt(3);
kappa = 3 - n_states;
lambda = alpha*alpha * (n_err_states+kappa) - n_err_states;
beta = -(1-(alpha*alpha));
w_m_0 = (lambda)/(n_err_states + lambda);
w_c_0 = (lambda/(n_err_states + lambda)) + (1 - (alpha*alpha) + beta);
w_i = 0.5/(n_err_states +lambda);
initialize = 1;
sqrt_lambda = (lambda+n_err_states);
if(q_s_c == VNULL)
{
q_s_c = v_get(4);
q_s_c->ve[0] = -0.020656;
q_s_c->ve[1] = 0.71468;
q_s_c->ve[2] = -0.007319;
q_s_c->ve[3] = 0.6991;
}
if(Torq_prev == VNULL)
{
Torq_prev = v_get(3);
v_zero(Torq_prev);
}
quat_normalize(q_s_c);
}
result = m_get(9,9);
m_zero(result);
result1 = m_get(n_err_states, n_err_states);
m_zero(result1);
if(x_m_o == VNULL)
{
x_m_o = v_get(n_states);
v_zero(x_m_o);
}
x = v_get(n_states);
v_zero(x);
x_err_priori = v_get(n_err_states);
v_zero(x_err_priori);
x_ang_vel = v_get(3);
v_zero(x_ang_vel);
sig_pt = m_get(n_states, n_err_states);
m_zero(sig_pt);
if (C == MNULL)
{
C = m_get(9, 12);
m_zero(C);
}
if (P_priori == MNULL)
{
P_priori = m_get(n_err_states, n_err_states);
m_zero(P_priori);
}
if (Q == MNULL)
{
Q = m_get(n_err_states, n_err_states);
m_ident(Q);
//
Q->me[0][0] = 0.0001;
Q->me[1][1] = 0.0001;
Q->me[2][2] = 0.0001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.000001;
Q->me[7][7] = 0.000001;
Q->me[8][8] = 0.000001;
Q->me[9][9] = 0.000001;
Q->me[10][10] = 0.000001;
Q->me[11][11] = 0.000001;
}
if( Pprev == MNULL)
{
Pprev = m_get(n_err_states, n_err_states);
m_ident(Pprev);
Pprev->me[0][0] = 1e-3;
Pprev->me[1][1] = 1e-3;
Pprev->me[2][2] = 1e-3;
Pprev->me[3][3] = 1e-3;
Pprev->me[4][4] = 1e-3;
Pprev->me[5][5] = 1e-3;
Pprev->me[6][6] = 1e-4;
Pprev->me[7][7] = 1e-4;
Pprev->me[8][8] = 1e-4;
Pprev->me[9][9] = 1e-3;
Pprev->me[10][10] = 1e-3;
Pprev->me[11][11] = 1e-3;
}
if (R == MNULL)
{
R = m_get(9,9);
m_ident(R);
R->me[0][0] = 0.034;
R->me[1][1] = 0.034;
R->me[2][2] = 0.034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.000012;
R->me[7][7] = 0.000012;
R->me[8][8] = 0.000012;
}
if(eclipse==0)
{
R->me[0][0] = 0.00034;
R->me[1][1] = 0.00034;
R->me[2][2] = 0.00034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;//0.000012;//0.0175;//1e-3;
Q->me[4][4] = 0.0001;//0.0175;//1e-3;
Q->me[5][5] = 0.0001;//0.0175;//1e-3;
Q->me[6][6] = 0.0000000001;//1e-6;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
else
{
R->me[0][0] = 0.34;
R->me[1][1] = 0.34;
R->me[2][2] = 0.34;
R->me[3][3] = 0.0027;
R->me[4][4] = 0.0027;
R->me[5][5] = 0.0027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.0000000001;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
if(omega_prev == VNULL)
{
omega_prev = v_get(3);
v_zero(omega_prev);
}
if(mag_vec_prev == VNULL)
{
mag_vec_prev = v_get(3);
v_zero(mag_vec_prev);
}
if(sun_vec_prev == VNULL)
{
sun_vec_prev = v_get(3);
v_zero(sun_vec_prev);
}
if (err_sig_pt_mat == MNULL)
{
err_sig_pt_mat = m_get(n_err_states, n_sig_pts);
m_zero(err_sig_pt_mat);
}
if(q_err_quat == VNULL)
{
q_err_quat = v_get(4);
// q_err_quat = v_resize(q_err_quat,4);
v_zero(q_err_quat);
}
if(err_vec == VNULL)
{
err_vec = v_get(3);
v_zero(err_vec);
}
v_temp = v_get(9);
v_resize(v_temp,3);
if(x_prev == VNULL)
{
x_prev = v_get(n_states);
v_zero(x_prev);
x_prev->ve[3] = 1;
quat_mul(x_prev,q_s_c,x_prev);
x_prev->ve[4] = 0.0;
x_prev->ve[5] = 0.0;
x_prev->ve[6] = 0.0;
x_prev->ve[7] = 0.0;
x_prev->ve[8] = 0.0;
x_prev->ve[9] = 0.0;
x_prev->ve[10] = 0.0;
x_prev->ve[11] = 0.0;
x_prev->ve[12] = 0.0;
}
sqrt_P = m_get(n_err_states, n_err_states);
m_zero(sqrt_P);
//result = m_resize(result, n_err_states, n_err_states);
result_larger = m_get(n_err_states, n_err_states);
int n, m;
for(n = 0; n < result->n; n++)
{
for(m = 0; m < result->m; m++)
{
result_larger->me[m][n] = result->me[m][n];
}
}
//v_resize(v_temp, n_err_states);
V_FREE(v_temp);
v_temp = v_get(n_err_states);
symmeig(Pprev, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_copy(Pprev, result1);
sm_mlt(sqrt_lambda, result1, result_larger);
catchall(CHfactor(result_larger), printerr(sim_time));
for(i=0; i<n_err_states; i++){
for(j=i+1; j<n_err_states; j++){
result_larger->me[i][j] = 0;
}
}
expandstate(result_larger, x_prev, sig_pt);
sig_vec_mat = m_get(n_states, n_sig_pts);
m_zero(sig_vec_mat);
for(j = 0; j<(n_err_states+1); j++)
{
for(i = 0; i<n_states; i++)
{
if(j==0)
{
sig_vec_mat->me[i][j] = x_prev->ve[i];
}
else if(j>0)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-1];
}
}
}
sm_mlt(-1,result_larger,result_larger);
expandstate(result_larger, x_prev, sig_pt);
for(j = (n_err_states+1); j<n_sig_pts; j++)
{
for(i = 0; i<n_states; i++)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-(n_err_states+1)];
}
}
single_sig_pt = v_get(n_states);
quat_rot_vec(q_s_c, Torq_ext);
for(j=0; j<(n_sig_pts); j++)
{
//v_temp = v_resize(v_temp,n_states);
V_FREE(v_temp);
v_temp = v_get(n_states);
get_col(sig_vec_mat, j, single_sig_pt);
v_copy(single_sig_pt, v_temp);
rk4(t, v_temp, h, Torq_prev);
set_col(sig_vec_mat, j, v_temp);
}
v_copy(Torq_ext, Torq_prev);
x_priori = v_get(n_states);
v_zero(x_priori);
v_resize(v_temp,n_states);
v_zero(v_temp);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp);
if(j == 0)
{
v_mltadd(x_priori, v_temp, w_m_0, x_priori);
}
else
{
v_mltadd(x_priori, v_temp, w_i, x_priori);
}
}
v_copy(x_priori, v_temp);
v_resize(v_temp,4);
quat_normalize(v_temp);//zaroori hai ye
for(i=0; i<4; i++)
{
x_priori->ve[i] = v_temp->ve[i];
}
v_resize(v_temp, n_states);
v_copy(x_priori, v_temp);
v_resize(v_temp, 4);
quat_inv(v_temp, v_temp);
for(i=0; i<3; i++)
{
x_ang_vel->ve[i] = x_priori->ve[i+4];
}
x_b_m = v_get(3);
v_zero(x_b_m);
x_b_g = v_get(3);
v_zero(x_b_g);
/////////////////////////check it!!!!!!!! checked... doesnt change much the estimate
for(i=0; i<3; i++)
{
x_b_m->ve[i] = x_priori->ve[i+7];
x_b_g->ve[i] = x_priori->ve[i+10];
}
v_temp2 = v_get(n_states);
v_zero(v_temp2);
for(j=0; j<n_sig_pts; j++)
{
v_resize(v_temp2, n_states);
get_col( sig_vec_mat, j, v_temp2);
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+4];
}
v_resize(v_temp2, 4);
quat_mul(v_temp2, v_temp, q_err_quat);
v_resize(q_err_quat, n_err_states);
v_sub(err_vec, x_ang_vel, err_vec);
for(i=3; i<6; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-3];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+7];
}
v_sub(err_vec, x_b_m, err_vec);
for(i=6; i<9; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-6];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+10];
}
v_sub(err_vec, x_b_g, err_vec);
for(i=9; i<12; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-9];
}
set_col(err_sig_pt_mat, j, q_err_quat);
if(j==0){
v_mltadd(x_err_priori, q_err_quat, w_m_0, x_err_priori);
}
else{
v_mltadd(x_err_priori, q_err_quat, w_i, x_err_priori);
}
}
v_resize(v_temp,n_err_states);
for (j=0;j<13;j++)
{
get_col(err_sig_pt_mat, j, v_temp);
v_sub(v_temp, x_err_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
}
else{
sm_mlt(w_i, result_larger, result_larger);
}
m_add(P_priori, result_larger, P_priori);
}
symmeig(P_priori, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_priori, Q, P_priori);
v_resize(v_temp,3);
meas = v_get(9);
if (!(is_vec_equal(sun_vec, sun_vec_prev)) /*&& (eclipse==0)*/ ){
update_sun_vec =1;
v_copy(sun_vec, sun_vec_prev);
v_copy(sun_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i = 0; i<3;i++){
meas->ve[i] = v_temp->ve[i];
}
}
else{
update_sun_vec =0;
for(i = 0; i<3;i++){
meas->ve[i] = 0;
}
}
if (!(is_vec_equal(mag_vec, mag_vec_prev)) ){
update_mag_vec =1;
v_copy(mag_vec, mag_vec_prev);
v_copy(mag_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i=3; i<6; i++){
meas->ve[i] = v_temp->ve[i-3];
}
}
else{
update_mag_vec =0;
for(i=3; i<6; i++){
meas->ve[i] = 0;//mag_vec_prev->ve[i-3];
}
}
if (!(is_vec_equal(omega, omega_prev) ) ){
update_omega =1;
v_copy(omega, omega_prev);
v_copy(omega, v_temp);
quat_rot_vec(q_s_c, v_temp);
for(i=6; i<9; i++){
meas->ve[i] = v_temp->ve[i-6];
}
}
else{
update_omega =0;
for(i=6; i<9; i++){
meas->ve[i] = 0;
}
}
v_resize(v_temp, 9);
v_resize(v_temp2, n_states);
v_temp3 = v_get(3);
Meas_err_mat = m_get(9, n_sig_pts);
m_zero(Meas_err_mat);
meas_priori = v_get(9);
v_zero(meas_priori);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp2);
if(update_omega){
for(i=6;i<9;i++){
v_temp->ve[i] = v_temp2->ve[i-2] + x_b_g->ve[i-6];
}
}
else{
for(i=6;i<9;i++){
v_temp->ve[i] = 0;
}
}
v_resize(v_temp2, 4);
if(update_sun_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = sun_vec_I->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp->ve[i] = v_temp3->ve[i];
}
}
else{
for(i=0;i<3;i++){
v_temp->ve[i] = 0;
}
}
if(update_mag_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = mag_vec_I->ve[i];
}
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp3->ve[i] = v_temp3->ve[i] + x_b_m->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=3;i<6;i++){
v_temp->ve[i] = v_temp3->ve[i-3];
}
}
else{
for(i=3;i<6;i++){
v_temp->ve[i] = 0;
}
}
set_col(Meas_err_mat, j, v_temp);
if(j==0){
v_mltadd(meas_priori, v_temp, w_m_0, meas_priori);
}
else{
v_mltadd(meas_priori, v_temp, w_i, meas_priori);
}
}
v_resize(v_temp, 9);
m_resize(result_larger, 9, 9);
m_zero(result_larger);
P_zz = m_get(9, 9);
m_zero(P_zz);
iP_vv = m_get(9, 9);
m_zero(iP_vv);
P_xz = m_get(n_err_states, 9);
m_zero(P_xz);
v_resize(v_temp2, n_err_states);
result1 = m_resize(result1,n_err_states,9);
for (j=0; j<n_sig_pts; j++)
{
get_col( Meas_err_mat, j, v_temp);
get_col( err_sig_pt_mat, j, v_temp2);
v_sub(v_temp, meas_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
get_dyad(v_temp2, v_temp, result1);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
sm_mlt(w_c_0, result1, result1);
}
else{
sm_mlt(w_i, result_larger, result_larger);
sm_mlt(w_i, result1, result1);
}
m_add(P_zz, result_larger, P_zz);
m_add(P_xz, result1, P_xz);
}
symmeig(P_zz, result_larger, v_temp);
i = 0;
for (j=0; j<9; j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_zz, R, P_zz);
m_inverse(P_zz, iP_vv);
K = m_get(n_err_states, 9);
m_zero(K);
m_mlt(P_xz, iP_vv, K);
if(x_posteriori_err == VNULL)
{
x_posteriori_err = v_get(n_err_states);
v_zero(x_posteriori_err);
}
v_resize(v_temp,9);
v_sub(meas, meas_priori, v_temp);
v_copy(v_temp, residual);
mv_mlt(K, v_temp, x_posteriori_err);
v_resize(v_temp2,3);
for(i=0;i<3;i++){
v_temp2->ve[i] = x_posteriori_err->ve[i];
}
for(i=4; i<n_states; i++){
x_prev->ve[i] = (x_posteriori_err->ve[i-1] + x_priori->ve[i]);
}
d_res = v_norm2(v_temp2);
v_resize(v_temp2,4);
if(d_res<=1 /*&& d_res!=0*/){
v_temp2->ve[0] = v_temp2->ve[0];
v_temp2->ve[1] = v_temp2->ve[1];
v_temp2->ve[2] = v_temp2->ve[2];
v_temp2->ve[3] = sqrt(1-d_res);
}
else//baad main daala hai
{
v_temp2->ve[0] = (v_temp2->ve[0])/(sqrt(1+d_res));
v_temp2->ve[1] = (v_temp2->ve[1])/(sqrt(1+d_res));
v_temp2->ve[2] = (v_temp2->ve[2])/(sqrt(1+d_res));
v_temp2->ve[3] = 1/sqrt(1 + d_res);
}
v_resize(x_posteriori_err, n_states);
for(i=(n_states-1); i>3; i--){
x_posteriori_err->ve[i] = x_posteriori_err->ve[i-1];
}
for(i=0; i<4; i++){
x_posteriori_err->ve[i] = v_temp2->ve[i];
}
quat_mul(v_temp2, x_priori, v_temp2);
for(i=0;i<4;i++){
x_prev->ve[i] = v_temp2->ve[i];
}
m_resize(result_larger, n_err_states, 9);
m_mlt(K, P_zz, result_larger);
result2 = m_get(9, n_err_states);
m_transp(K,result2);
m_resize(result1, n_err_states, n_err_states);
m_mlt(result_larger, result2, result1);
v_resize(v_temp, n_err_states);
m_sub(P_priori, result1, Pprev);
symmeig(Pprev, result1 , v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
v_copy(x_prev, v_temp);
v_resize(v_temp,4);
v_copy(x_prev, v_temp2);
v_resize(v_temp2,4);
v_copy(x_prev, x_m_o);
//v_resize(x_m_o, 4);
v_resize(v_temp,3);
quat_inv(q_s_c, v_temp2);
v_copy( x_prev, state);
quat_mul(state, v_temp2, state);
for(i=0; i<3; i++){
v_temp->ve[i] = state->ve[i+4];
}
quat_rot_vec(v_temp2, v_temp);
for(i=0; i<3; i++){
state->ve[i+4] = v_temp->ve[i];
}
v_copy( x_posteriori_err, st_error);
iter_num++;
V_FREE(x);
V_FREE(x_priori);
V_FREE(x_err_priori);
V_FREE(single_sig_pt);
V_FREE(v_temp);
V_FREE(q_err_quat);
V_FREE(err_vec);
V_FREE(v_temp2);
V_FREE(x_ang_vel);
V_FREE(meas);
V_FREE(meas_priori);
V_FREE(v_temp3);
V_FREE(x_posteriori_err);
V_FREE(x_b_m);
V_FREE(x_b_g);
M_FREE(sqrt_P);
M_FREE(P);
M_FREE(P_priori);
M_FREE(sig_pt);
M_FREE(sig_vec_mat);
M_FREE(err_sig_pt_mat);
M_FREE(result);
M_FREE(result_larger);
M_FREE(result1);
M_FREE(Meas_err_mat);
M_FREE(P_zz);
M_FREE(iP_vv);
M_FREE(P_xz);
M_FREE(K);
M_FREE(result2);
M_FREE(result3);
}
/* q = q * qi */
void quat_mul_self(union quat *q, const union quat *qi)
{
union quat qo;
quat_mul(&qo, q, qi);
*q = qo;
}
/* q = qi * q */
void quat_mul_self_right(const union quat *qi, union quat *q)
{
union quat qo;
quat_mul(&qo, qi, q);
*q = qo;
}
int32_t pivot_lookat(struct Pivot* pivot, const Vec3f target) {
int32_t result = -1;
Vec4f right_axis = RIGHT_AXIS;
Vec4f up_axis = UP_AXIS;
Vec4f forward_axis = FORWARD_AXIS;
struct Pivot world_pivot;
pivot_combine(pivot->parent, pivot, &world_pivot);
Vec4f target_direction;
vec_sub(target, world_pivot.position, target_direction);
float dot_yaw = vec_dot(target_direction, forward_axis);
Quat rotation = {0};
if( fabs(dot_yaw + 1.0f) < CUTE_EPSILON ) {
// vector a and b point exactly in the opposite direction,
// so it is a 180 degrees turn around the up-axis
Quat rotation = {0};
quat_from_axis_angle(up_axis, PI, rotation);
quat_mul(world_pivot.orientation, rotation, rotation);
} else if( fabs(dot_yaw - (1.0f)) < CUTE_EPSILON ) {
// vector a and b point exactly in the same direction
// so we return the identity quaternion
quat_copy(world_pivot.orientation, rotation);
} else {
// - I look at the target by turning the pivot orientation using only
// yaw and pitch movement
// - I always rotate the pivot from its initial orientation (up and forward
// basis vectors like initialized above), so this does not incrementally
// advance the orientation
quat_identity(rotation);
// - to find the amount of yaw I project the target_direction into the
// up_axis plane, resulting in up_projection which is a vector that
// points from the up_axis plane to the tip of the target_direction
Vec4f up_projection = {0};
vec_mul1f(up_axis, vec_dot(target_direction, up_axis), up_projection);
// - so then by subtracting the up_projection from the target_direction,
// I get a vector lying in the up_axis plane, pointing towards the target
Vec4f yaw_direction = {0};
vec_sub(target_direction, up_projection, yaw_direction);
// - angle between yaw_direction and forward_axis is the amount of yaw we
// need to point the forward_axis toward the target
float yaw = 0.0f;
vec_angle(yaw_direction, forward_axis, &yaw);
log_assert( ! isnan(yaw),
"vec_angle(%f %f %f, %f %f %f, %f);\n",
yaw_direction[0], yaw_direction[1], yaw_direction[2],
forward_axis[0], forward_axis[1], forward_axis[2],
yaw );
// - I have to compute the cross product between yaw_direction and
// forward_axis and use the resulting yaw_axis
Vec4f yaw_axis = {0};
vec_cross(yaw_direction, forward_axis, yaw_axis);
if( vec_nullp(yaw_axis) ) {
vec_copy4f(up_axis, yaw_axis);
}
// - compute the yaw rotation
Quat yaw_rotation = {0};
quat_from_axis_angle(yaw_axis, yaw, yaw_rotation);
// - to compute, just as with the yaw, I want an axis that lies on the plane that
// is spanned in this case by the right_axis, when the camera points toward the
// target
// - I could compute an axis, but I already have a direction vector that points
// toward the target, the yaw_direction, I just have to normalize it to make it
// an axis (and put the result in forward_axis, since it now is the forward_axis
// of the yaw turned camera)
Vec4f yaw_forward_axis = {0};
vec_normalize(yaw_direction, yaw_forward_axis);
// - then use the new forward axis with the old target_direction to compute the angle
// between those
float pitch = 0.0f;
vec_angle(target_direction, yaw_forward_axis, &pitch);
log_assert( ! isnan(pitch),
"vec_angle(%f %f %f, %f %f %f, %f);\n",
target_direction[0], target_direction[1], target_direction[2],
yaw_forward_axis[0], yaw_forward_axis[1], yaw_forward_axis[2],
pitch );
// - and just as in the yaw case we compute an rotation pitch_axis
Vec4f pitch_axis = {0};
vec_cross(target_direction, yaw_forward_axis, pitch_axis);
if( vec_nullp(pitch_axis) ) {
vec_copy4f(right_axis, pitch_axis);
}
// - and finally compute the pitch rotation and combine it with the yaw_rotation
// in the same step
Quat pitch_rotation;
quat_from_axis_angle(pitch_axis, pitch, pitch_rotation);
Quat yaw_pitch_rotation;
quat_mul(yaw_rotation, pitch_rotation, yaw_pitch_rotation);
Quat inverted_orientation = {0};
quat_invert(world_pivot.orientation, inverted_orientation);
// - the int32_t I want to return indicates the cameras 'flip' status, that is, it is
// one when the camera angle was pitched so much that it flipped over and its
// up axis is now pointing downwards
// - to find out if I am flipped over, I compute the flipped up_axis called
// flip_axis and then use the dot product between the flip_axis and up_axis
// to decide if I am flipped
Vec4f flip_axis = {0};
vec_rotate(up_axis, inverted_orientation, flip_axis);
vec_rotate(flip_axis, yaw_pitch_rotation, flip_axis);
float dot_pitch = vec_dot(up_axis, flip_axis);
Vec4f target_axis = {0};
vec_normalize(target_direction, target_axis);
// - check if we are flipped and if we are, set result to 1 meaning we are flipped
// - turn the camera around PI so that we can continue pitching, otherwise we just get
// stuck when trying to flip the camera over
if( dot_pitch < 0.0f ) {
result = 1;
quat_from_axis_angle(target_axis, PI, rotation);
quat_mul(yaw_pitch_rotation, rotation, yaw_pitch_rotation);
}
quat_copy(yaw_pitch_rotation, rotation);
}
if( ! isnan(rotation[0]) &&
! isnan(rotation[1]) &&
! isnan(rotation[2]) &&
! isnan(rotation[3]) )
{
quat_copy(rotation, pivot->orientation);
}
pivot->eye_distance = vec_length(target_direction);
return result;
}
bool arcball_event(struct Arcball* arcball, SDL_Event event) {
static int32_t mouse_down = 0;
static const float rotation_slowness_factor = 0.25f;
static int32_t next_flipped = 0;
// - arcball rotation is performed by dragging the mouse, so I just keep track of when
// a mouse button is pressed and released between calls to this function by setting a
// static variable mouse_down to the button number when a button is pressed and back
// to 0 when that button is released
if( event.type == SDL_MOUSEBUTTONDOWN && mouse_down == 0 ) {
mouse_down = event.button.button;
} else if( event.type == SDL_MOUSEBUTTONUP && mouse_down == event.button.button ) {
arcball->flipped = next_flipped;
mouse_down = 0;
}
if( mouse_down == arcball->translate_button && event.type == SDL_MOUSEMOTION ) {
SDL_MouseMotionEvent mouse = event.motion;
float eye_distance = arcball->camera.pivot.eye_distance;
// - when an mouse motion event occurs, and mouse_down to the translation_button so we compute
// a camera translation
// - the camera should pan around on the x-z-plane, keeping its height and orientation
Quat inverted_orientation = {0};
quat_invert(arcball->camera.pivot.orientation, inverted_orientation);
// - the sideways translation is computed by taking the right_axis and orienting it with
// the cameras orientation, the way I set up the lookat implementation this should always
// result in a vector parallel to the x-z-plane
Vec4f right_axis = RIGHT_AXIS;
vec_rotate4f(right_axis, inverted_orientation, right_axis);
if( mouse.xrel != 0 ) {
// - then we'll just multiply the resulting axis with the mouse x relative movement, inversely
// scaled by how far we are away from what we are looking at (farer means faster, nearer
// means slower), the translation_factor is just a value that felt good when this was implemented
Vec4f x_translation = {0};
vec_mul1f(right_axis, (float)mouse.xrel/arcball->translate_factor*eye_distance, x_translation);
// - finally just add the x_translation to the target and position so that the whole arcball moves
vec_add(arcball->target, x_translation, arcball->target);
vec_add(arcball->camera.pivot.position, x_translation, arcball->camera.pivot.position);
}
// - the z translation can't be done along the orientated forward axis because that would include
// the camera pitch, here, same as above, we need an axis that is parallel to the x-z-plane
Vec4f up_axis = UP_AXIS;
if( mouse.yrel != 0 ) {
// - luckily such an axis is easily computed from the crossproduct of the orientated right_axis and
// the default up_axis, the result is an axis pointing in the direction of the cameras forward axis,
// while still being parallel to the x-z-plane
Vec4f forward_axis;
vec_cross(right_axis, up_axis, forward_axis);
// - same as above
Vec4f z_translation;
vec_mul1f(forward_axis, (float)mouse.yrel/arcball->translate_factor*eye_distance, z_translation);
// - dito
vec_add(arcball->target, z_translation, arcball->target);
vec_add(arcball->camera.pivot.position, z_translation, arcball->camera.pivot.position);
}
} else if( mouse_down == arcball->rotate_button && event.type == SDL_MOUSEMOTION ) {
SDL_MouseMotionEvent mouse = event.motion;
// - above case was translation, this is an rotation occuring: mouse_down is equal to the
// rotation_button and the event is a mouse motion
// - the camera needs to rotate around the target while keeping its height, orientation and
// without _any_ rolling
// - we want only yaw and pitch movement
// - yaw is easy, just use the fixed up_axis and create a rotation the rotates around it
// by the mouse x relative movement (converted to radians)
// - the flipped value indicates if the camera is flipped over, so we'll just use that to
// change the sign of the yaw to make the mouse movement on the screen always correctly
// relates to the movement of the rotation
Vec4f up_axis = UP_AXIS;
Quat yaw_rotation = {0};
quat_from_axis_angle(up_axis, arcball->flipped * PI/180 * mouse.xrel * rotation_slowness_factor, yaw_rotation);
// - pitch is a little more involved, I need to compute the orientated right axis and use
// that to compute the pitch_rotation
Quat inverted_orientation = {0};
quat_invert(arcball->camera.pivot.orientation, inverted_orientation);
Vec4f right_axis = RIGHT_AXIS;
vec_rotate4f(right_axis, inverted_orientation, right_axis);
Quat pitch_rotation = {0};
quat_from_axis_angle(right_axis, -PI/180 * mouse.yrel * rotation_slowness_factor, pitch_rotation);
// - combine yaw and pitch into a single rotation
Quat rotation = {0};
quat_mul(yaw_rotation, pitch_rotation, rotation);
// - orbit is the position translated to the coordinate root
// - the yaw and pitch rotation is applied to the orbit
// - orbit is translated back and replaces the camera position
Vec4f orbit = {0};
vec_sub(arcball->camera.pivot.position, arcball->target, orbit);
vec_rotate4f(orbit, rotation, orbit);
vec_add(arcball->target, orbit, arcball->camera.pivot.position);
// - after updating the position we just call lookat to compute the new
// orientation, and also set the flipped state
next_flipped = pivot_lookat(&arcball->camera.pivot, arcball->target);
}
if( event.type == SDL_MOUSEWHEEL ) {
SDL_MouseWheelEvent wheel = event.wheel;
// - zooming when mouse wheel event happens
float* eye_distance = &arcball->camera.pivot.eye_distance;
if( (*eye_distance > arcball->camera.frustum.z_near || wheel.y < 0) &&
(*eye_distance < arcball->camera.frustum.z_far || wheel.y > 0))
{
// - just going back and forth along the oriented forward axis, using wheel
// y motion inversly scaled by the eye_distance, similar to what is done
// for the translation above (farer == faster zoom, nearer == slower zoom)
Quat inverted_orientation = {0};
quat_invert(arcball->camera.pivot.orientation, inverted_orientation);
Vec4f forward_axis = FORWARD_AXIS;
vec_rotate4f(forward_axis, inverted_orientation, forward_axis);
Vec4f zoom = {0};
vec_mul1f(forward_axis, wheel.y/arcball->zoom_factor*(*eye_distance), zoom);
vec_add(arcball->camera.pivot.position, zoom, arcball->camera.pivot.position);
// - eye_distance is kept in camera state, so we need to update it here
*eye_distance = vlength(arcball->camera.pivot.position);
}
}
return true;
}
void expandstate(MAT *q_x_err, VEC *x_full, MAT *q_x_expanded)
{
VEC *q_temp, *q_err_vec, *q_err_quat;
double temp, n_err_states, n_states;
int i, j;
n_states = 7+6;
n_err_states = 6+6;
q_temp = v_get(4);
v_zero(q_temp);
q_err_vec = v_get(3);
q_err_quat = v_get(4);
for(j=0; j<n_err_states; j++)
{
for(i=0; i<3; i++)
{
q_err_vec->ve[i] = q_x_err->me[i][j];
}
temp = v_norm2(q_err_vec);
if(temp<=1 ){
q_err_quat->ve[0] = q_err_vec->ve[0];
q_err_quat->ve[1] = q_err_vec->ve[1];
q_err_quat->ve[2] = q_err_vec->ve[2];
q_err_quat->ve[3] = sqrt(1-temp);
}
else//baad main daala hai
{
q_err_quat->ve[0] = (q_err_vec->ve[0])/(sqrt(1+temp));
q_err_quat->ve[1] = (q_err_vec->ve[1])/(sqrt(1+temp));
q_err_quat->ve[2] = (q_err_vec->ve[2])/(sqrt(1+temp));
q_err_quat->ve[3] = 1/sqrt(1+temp);
}
quat_mul(q_err_quat, x_full, q_temp);
for(i=0; i<4; i++)
{
q_x_expanded->me[i][j] = q_temp->ve[i];
}
for(i=4; i<7; i++)
{
q_x_expanded->me[i][j] = q_x_err->me[i-1][j] + x_full->ve[i];
}
for(i=7; i<10; i++)
{
q_x_expanded->me[i][j] = q_x_err->me[i-1][j] + x_full->ve[i];
}
for(i=10; i<13; i++)
{
q_x_expanded->me[i][j] = q_x_err->me[i-1][j] + x_full->ve[i];
}
}
//
v_free(q_temp);
v_free(q_err_vec);
v_free(q_err_quat);
}
/**
* Rotate this object by the given quaternion.
**/
void
object_rotate(object_t *object, quat_t *quat)
{
object_invalidate_transform_cache(object);
quat_mul(quat, &object->rot, &object->rot);
}
