Skip to content

GitLab

Commit d435994e authored by POLLIEN Baptiste's avatar POLLIEN Baptiste
Browse files

Verification of the conversion function rmat to quat

parent d6dadeee
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 713 additions and 6 deletions
sw/airborne/.frama-c/wp/script/float_quat_of_rmat_a00_max_ensures.json 0 → 100644
View file @ d435994e
[ { "header": "Definition", "tactic": "Wp.unfold", "params": {},
"select": { "select": "inside-goal", "occur": 0,
"target": "(L_l_FloatQuat_of_RMat_0_max_t Mf32_0 rm_0)",
"pattern": "L_l_FloatQuat_of_RMat_0_max_t$Mf32" },
"children": { "Unfold 'L_l_FloatQuat_of_RMat_0_max_t'": [ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-goal",
"occur": 0,
"target": "(L_l_RMat_of_FloatRMat Mf32_0 rm_0)",
"pattern": "L_l_RMat_of_FloatRMat$Mf32$rm" },
"children":
{ "Unfold 'L_l_RMat_of_FloatRMat'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-goal",
"occur": 0,
"target": "let a_0 = (shiftfield_F6_FloatRMat_m rm_0) in\n(L_transpose\n {\n F9_RealRMat_s_a00 = Mf32_0[(shift_float32 a_0 0)] ;\n F9_RealRMat_s_a01 = Mf32_0[(shift_float32 a_0 1)] ;\n F9_RealRMat_s_a02 = Mf32_0[(shift_float32 a_0 2)] ;\n F9_RealRMat_s_a10 = Mf32_0[(shift_float32 a_0 3)] ;\n F9_RealRMat_s_a11 = Mf32_0[(shift_float32 a_0 4)] ;\n F9_RealRMat_s_a12 = Mf32_0[(shift_float32 a_0 5)] ;\n F9_RealRMat_s_a20 = Mf32_0[(shift_float32 a_0 6)] ;\n F9_RealRMat_s_a21 = Mf32_0[(shift_float32 a_0 7)] ;\n F9_RealRMat_s_a22 = Mf32_0[(shift_float32 a_0 8)]\n })",
"pattern": "L_transpose{RealRMat_s}[][][][][]" },
"children":
{ "Unfold 'L_transpose'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-goal",
"occur": 0,
"target": "let a_0 = (shiftfield_F6_FloatRMat_m rm_0) in\n(L_l_FloatQuat_of_RMat_0_max_1_\n {\n F9_RealRMat_s_a00 = Mf32_0[(shift_float32 a_0 0)] ;\n F9_RealRMat_s_a01 = Mf32_0[(shift_float32 a_0 3)] ;\n F9_RealRMat_s_a02 = Mf32_0[(shift_float32 a_0 6)] ;\n F9_RealRMat_s_a10 = Mf32_0[(shift_float32 a_0 1)] ;\n F9_RealRMat_s_a11 = Mf32_0[(shift_float32 a_0 4)] ;\n F9_RealRMat_s_a12 = Mf32_0[(shift_float32 a_0 7)] ;\n F9_RealRMat_s_a20 = Mf32_0[(shift_float32 a_0 2)] ;\n F9_RealRMat_s_a21 = Mf32_0[(shift_float32 a_0 5)] ;\n F9_RealRMat_s_a22 = Mf32_0[(shift_float32 a_0 8)]\n })",
"pattern": "L_l_FloatQuat_of_RMat_0_max_1_{RealRMat_s}" },
"children":
{ "Unfold 'L_l_FloatQuat_of_RMat_0_max_1_'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "clause-goal",
"target": "let a_0 = (shiftfield_F6_FloatRMat_m rm_0) in\nlet r_0 =\n (\\sqrt\n (1+Mf32_0[(shift_float32 a_0 0)]-Mf32_0[(shift_float32 a_0 4)]-\n Mf32_0[(shift_float32 a_0 8)])) in\nlet r_1 = (2*r_0) in\n(EqS10_RealQuat_s\n {\n F10_RealQuat_s_qi =\n (Mf32_0[(shift_float32 a_0 5)]-Mf32_0[(shift_float32 a_0 7)]) div r_1 ;\n F10_RealQuat_s_qx = 1/2*r_0 ;\n F10_RealQuat_s_qy =\n (Mf32_0[(shift_float32 a_0 1)]+Mf32_0[(shift_float32 a_0 3)]) div r_1 ;\n F10_RealQuat_s_qz =\n (Mf32_0[(shift_float32 a_0 2)]+Mf32_0[(shift_float32 a_0 6)]) div r_1\n } (L_l_Quat_of_FloatQuat Mf32_0 q_0))",
"pattern": "EqS10_RealQuat_s{RealQuat_s}L_l_Quat_of_FloatQuat" },
"children":
{ "Unfold 'EqS10_RealQuat_s'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-goal",
"occur": 0,
"target": "(L_l_Quat_of_FloatQuat Mf32_0 q_0)",
"pattern": "L_l_Quat_of_FloatQuat$Mf32$q" },
"children":
{ "Unfold 'L_l_Quat_of_FloatQuat'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-step",
"at": 3,
"kind": "have",
"occur": 0,
"target": "(L_trace_2_ Mf32_1 rm_0)",
"pattern": "L_trace_2_$Mf32$rm" },
"children":
{ "Unfold 'L_trace_2_'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-step",
"at": 3,
"kind": "have",
"occur": 0,
"target": "(L_trace_1_ (L_l_RMat_of_FloatRMat Mf32_1 rm_0))",
"pattern": "L_trace_1_L_l_RMat_of_FloatRMat$Mf32" },
"children":
{ "Unfold 'L_trace_1_'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-step",
"at": 3,
"kind": "have",
"occur": 0,
"target": "(L_l_RMat_of_FloatRMat Mf32_1 rm_0)",
"pattern": "L_l_RMat_of_FloatRMat$Mf32$rm" },
"children":
{ "Unfold 'L_l_RMat_of_FloatRMat'":
[ { "prover": "Alt-Ergo:2.3.3",
"verdict": "valid",
"time": 0.9272,
"steps": 383 } ] } } ] } } ] } } ] } } ] } } ] } } ] } } ] } } ] } } ]
sw/airborne/.frama-c/wp/script/float_quat_of_rmat_a00_max_ensures_2.json 0 → 100644
View file @ d435994e
[ { "prover": "Z3:4.8.6:noBV", "verdict": "timeout", "time": 10. },
{ "prover": "Z3:4.8.6:counterexamples", "verdict": "timeout", "time": 10. },
{ "prover": "Z3:4.8.6", "verdict": "timeout", "time": 10. },
{ "prover": "CVC4:1.9-prerelease:strings+counterexamples",
"verdict": "timeout", "time": 10. },
{ "prover": "Alt-Ergo:2.3.3", "verdict": "timeout", "time": 10. } ]
sw/airborne/.frama-c/wp/script/float_quat_of_rmat_a11_max_ensures.json 0 → 100644
View file @ d435994e
[ { "header": "Definition", "tactic": "Wp.unfold", "params": {},
"select": { "select": "inside-goal", "occur": 0,
"target": "(L_l_FloatQuat_of_RMat_1_max_t Mf32_0 rm_0)",
"pattern": "L_l_FloatQuat_of_RMat_1_max_t$Mf32" },
"children": { "Unfold 'L_l_FloatQuat_of_RMat_1_max_t'": [ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-goal",
"occur": 0,
"target": "(L_transpose (L_l_RMat_of_FloatRMat Mf32_0 rm_0))",
"pattern": "L_transposeL_l_RMat_of_FloatRMat" },
"children":
{ "Unfold 'L_transpose'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-goal",
"occur": 0,
"target": "let a_0 = (L_l_RMat_of_FloatRMat Mf32_0 rm_0) in\n(L_l_FloatQuat_of_RMat_1_max_1_\n {\n a_0 with\n F9_RealRMat_s_a01 = a_0.F9_RealRMat_s_a10 ;\n F9_RealRMat_s_a02 = a_0.F9_RealRMat_s_a20 ;\n F9_RealRMat_s_a10 = a_0.F9_RealRMat_s_a01 ;\n F9_RealRMat_s_a12 = a_0.F9_RealRMat_s_a21 ;\n F9_RealRMat_s_a20 = a_0.F9_RealRMat_s_a02 ;\n F9_RealRMat_s_a21 = a_0.F9_RealRMat_s_a12\n })",
"pattern": "L_l_FloatQuat_of_RMat_1_max_1_{RealRMat_s}" },
"children":
{ "Unfold 'L_l_FloatQuat_of_RMat_1_max_1_'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "clause-goal",
"target": "let a_0 = (L_l_RMat_of_FloatRMat Mf32_0 rm_0) in\nlet r_0 =\n (\\sqrt\n (1+a_0.F9_RealRMat_s_a11-a_0.F9_RealRMat_s_a00-a_0.F9_RealRMat_s_a22)) in\nlet r_1 = (2*r_0) in\n(EqS10_RealQuat_s\n {\n F10_RealQuat_s_qi =\n (a_0.F9_RealRMat_s_a20-a_0.F9_RealRMat_s_a02) div r_1 ;\n F10_RealQuat_s_qx =\n (a_0.F9_RealRMat_s_a01+a_0.F9_RealRMat_s_a10) div r_1 ;\n F10_RealQuat_s_qy = 1/2*r_0 ;\n F10_RealQuat_s_qz =\n (a_0.F9_RealRMat_s_a12+a_0.F9_RealRMat_s_a21) div r_1\n } (L_l_Quat_of_FloatQuat Mf32_0 q_0))",
"pattern": "EqS10_RealQuat_s{RealQuat_s}L_l_Quat_of_FloatQuat" },
"children":
{ "Unfold 'EqS10_RealQuat_s'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-goal",
"occur": 0,
"target": "(L_l_RMat_of_FloatRMat Mf32_0 rm_0)",
"pattern": "L_l_RMat_of_FloatRMat$Mf32$rm" },
"children":
{ "Unfold 'L_l_RMat_of_FloatRMat'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-goal",
"occur": 0,
"target": "(L_l_Quat_of_FloatQuat Mf32_0 q_0)",
"pattern": "L_l_Quat_of_FloatQuat$Mf32$q" },
"children":
{ "Unfold 'L_l_Quat_of_FloatQuat'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-step",
"at": 3,
"kind": "have",
"occur": 0,
"target": "(L_trace_2_ Mf32_1 rm_0)",
"pattern": "L_trace_2_$Mf32$rm" },
"children":
{ "Unfold 'L_trace_2_'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-step",
"at": 3,
"kind": "have",
"occur": 0,
"target": "(L_trace_1_ (L_l_RMat_of_FloatRMat Mf32_1 rm_0))",
"pattern": "L_trace_1_L_l_RMat_of_FloatRMat$Mf32" },
"children":
{ "Unfold 'L_trace_1_'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-step",
"at": 3,
"kind": "have",
"occur": 0,
"target": "(L_l_RMat_of_FloatRMat Mf32_1 rm_0)",
"pattern": "L_l_RMat_of_FloatRMat$Mf32$rm" },
"children":
{ "Unfold 'L_l_RMat_of_FloatRMat'":
[ { "prover": "Alt-Ergo:2.3.3",
"verdict": "valid",
"time": 0.4666,
"steps": 385 } ] } } ] } } ] } } ] } } ] } } ] } } ] } } ] } } ] } } ]
sw/airborne/.frama-c/wp/script/float_quat_of_rmat_a22_max_ensures.json 0 → 100644
View file @ d435994e
[ { "header": "Definition", "tactic": "Wp.unfold", "params": {},
"select": { "select": "inside-goal", "occur": 0,
"target": "(L_l_FloatQuat_of_RMat_2_max_t Mf32_0 rm_0)",
"pattern": "L_l_FloatQuat_of_RMat_2_max_t$Mf32" },
"children": { "Unfold 'L_l_FloatQuat_of_RMat_2_max_t'": [ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-goal",
"occur": 0,
"target": "(L_transpose (L_l_RMat_of_FloatRMat Mf32_0 rm_0))",
"pattern": "L_transposeL_l_RMat_of_FloatRMat" },
"children":
{ "Unfold 'L_transpose'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-step",
"at": 3,
"kind": "have",
"occur": 0,
"target": "(L_trace_2_ Mf32_1 rm_0)",
"pattern": "L_trace_2_$Mf32$rm" },
"children":
{ "Unfold 'L_trace_2_'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-step",
"at": 3,
"kind": "have",
"occur": 0,
"target": "(L_trace_1_ (L_l_RMat_of_FloatRMat Mf32_1 rm_0))",
"pattern": "L_trace_1_L_l_RMat_of_FloatRMat$Mf32" },
"children":
{ "Unfold 'L_trace_1_'":
[ { "prover": "Alt-Ergo:2.3.3",
"verdict": "valid",
"time": 0.6638,
"steps": 372 } ] } } ] } } ] } } ] } } ]
sw/airborne/.frama-c/wp/script/float_quat_of_rmat_a22_max_ensures_2.json 0 → 100644
View file @ d435994e
[ { "prover": "Z3:4.8.6:noBV", "verdict": "timeout", "time": 10. },
{ "prover": "Z3:4.8.6:counterexamples", "verdict": "timeout", "time": 10. },
{ "prover": "Z3:4.8.6", "verdict": "timeout", "time": 10. },
{ "prover": "CVC4:1.9-prerelease:strings+counterexamples",
"verdict": "timeout", "time": 10. },
{ "prover": "Alt-Ergo:2.3.3", "verdict": "timeout", "time": 10. } ]
sw/airborne/.frama-c/wp/script/float_quat_of_rmat_complete_a00_max_a11_max_a22_max_trace.json 0 → 100644
View file @ d435994e
[ { "prover": "Z3:4.8.6:noBV", "verdict": "timeout", "time": 10. },
{ "prover": "Z3:4.8.6:counterexamples", "verdict": "timeout", "time": 10. },
{ "prover": "Z3:4.8.6", "verdict": "timeout", "time": 10. },
{ "prover": "CVC4:1.9-prerelease:strings+counterexamples",
"verdict": "timeout", "time": 10. },
{ "prover": "Alt-Ergo:2.3.3", "verdict": "timeout", "time": 10. } ]
sw/airborne/.frama-c/wp/script/float_quat_of_rmat_trace_pos_ensures.json 0 → 100644
View file @ d435994e
[ { "header": "Definition", "tactic": "Wp.unfold", "params": {},
"select": { "select": "inside-goal", "occur": 0,
"target": "(L_l_FloatQuat_of_RMat_trace_pos_t Mf32_0 rm_0)",
"pattern": "L_l_FloatQuat_of_RMat_trace_pos_t" },
"children": { "Unfold 'L_l_FloatQuat_of_RMat_trace_pos_t'": [ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-goal",
"occur": 0,
"target": "(L_l_FloatQuat_of_RMat_trace_pos_1_\n (L_transpose (L_l_RMat_of_FloatRMat Mf32_0 rm_0)))",
"pattern": "L_l_FloatQuat_of_RMat_trace_pos_1_" },
"children":
{ "Unfold 'L_l_FloatQuat_of_RMat_trace_pos_1_'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-goal",
"occur": 0,
"target": "(L_transpose (L_l_RMat_of_FloatRMat Mf32_0 rm_0))",
"pattern": "L_transposeL_l_RMat_of_FloatRMat" },
"children":
{ "Unfold 'L_transpose'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-goal",
"occur": 0,
"target": "(L_l_Quat_of_FloatQuat Mf32_0 q_0)",
"pattern": "L_l_Quat_of_FloatQuat$Mf32$q" },
"children":
{ "Unfold 'L_l_Quat_of_FloatQuat'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-goal",
"occur": 0,
"target": "let a_0 = (L_l_RMat_of_FloatRMat Mf32_0 rm_0) in\n(L_trace_1_\n {\n a_0 with\n F9_RealRMat_s_a01 = a_0.F9_RealRMat_s_a10 ;\n F9_RealRMat_s_a02 = a_0.F9_RealRMat_s_a20 ;\n F9_RealRMat_s_a10 = a_0.F9_RealRMat_s_a01 ;\n F9_RealRMat_s_a12 = a_0.F9_RealRMat_s_a21 ;\n F9_RealRMat_s_a20 = a_0.F9_RealRMat_s_a02 ;\n F9_RealRMat_s_a21 = a_0.F9_RealRMat_s_a12\n })",
"pattern": "L_trace_1_{RealRMat_s}.F9_RealRMat_s_a00" },
"children":
{ "Unfold 'L_trace_1_'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "clause-goal",
"target": "let a_0 = (L_l_RMat_of_FloatRMat Mf32_0 rm_0) in\nlet r_0 =\n (1/2\n div (\\sqrt\n (1+a_0.F9_RealRMat_s_a00+a_0.F9_RealRMat_s_a11+\n a_0.F9_RealRMat_s_a22))) in\n(EqS10_RealQuat_s\n {\n F10_RealQuat_s_qi = 1/4 div r_0 ;\n F10_RealQuat_s_qx = (a_0.F9_RealRMat_s_a12-a_0.F9_RealRMat_s_a21)*r_0 ;\n F10_RealQuat_s_qy = (a_0.F9_RealRMat_s_a20-a_0.F9_RealRMat_s_a02)*r_0 ;\n F10_RealQuat_s_qz = (a_0.F9_RealRMat_s_a01-a_0.F9_RealRMat_s_a10)*r_0\n }\n {\n F10_RealQuat_s_qi = Mf32_0[(shiftfield_F4_FloatQuat_qi q_0)] ;\n F10_RealQuat_s_qx = Mf32_0[(shiftfield_F4_FloatQuat_qx q_0)] ;\n F10_RealQuat_s_qy = Mf32_0[(shiftfield_F4_FloatQuat_qy q_0)] ;\n F10_RealQuat_s_qz = Mf32_0[(shiftfield_F4_FloatQuat_qz q_0)]\n })",
"pattern": "EqS10_RealQuat_s{RealQuat_s}{RealQuat_s}" },
"children":
{ "Unfold 'EqS10_RealQuat_s'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-goal",
"occur": 0,
"target": "(L_l_RMat_of_FloatRMat Mf32_0 rm_0)",
"pattern": "L_l_RMat_of_FloatRMat$Mf32$rm" },
"children":
{ "Unfold 'L_l_RMat_of_FloatRMat'":
[ { "header": "Definition",
"tactic": "Wp.unfold",
"params": {},
"select":
{ "select": "inside-step",
"at": 3,
"kind": "have",
"occur": 0,
"target": "(L_trace_2_ Mf32_1 rm_0)",
"pattern": "L_trace_2_$Mf32$rm" },
"children":
{ "Unfold 'L_trace_2_'":
[ { "prover": "Alt-Ergo:2.3.3",
"verdict": "valid",
"time": 4.6223,
"steps": 1005 } ] } } ] } } ] } } ] } } ] } } ] } } ] } } ] } } ]
sw/airborne/math/pprz_algebra_float.h
View file @ d435994e
......@@ -878,14 +878,38 @@ extern void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect
extern void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov);
/// Quaternion from rotation matrix.
/*
/*@
requires valid_FloatQuat(q);
requires rvalid_FloatRMat(rm);
requires rotation_matrix(l_RMat_of_FloatRMat(rm));
requires \separated(rm, q);
ensures unary_quaterion(q);
ensures transpose(l_RMat_of_FloatRMat(rm)) == l_RMat_of_FloatQuat(q);
assigns *q;
behavior trace_pos:
assumes trace(rm) > 0;
ensures l_FloatQuat_of_RMat_trace_pos_t(rm) == l_Quat_of_FloatQuat(q);
behavior a00_max:
assumes trace(rm) <= 0
&& RMAT_ELMT(*rm, 0, 0) > RMAT_ELMT(*rm, 1, 1)
&& RMAT_ELMT(*rm, 0, 0) > RMAT_ELMT(*rm, 2, 2);
ensures l_FloatQuat_of_RMat_0_max_t(rm) == l_Quat_of_FloatQuat(q);
behavior a11_max:
assumes trace(rm) <= 0
&& RMAT_ELMT(*rm, 1, 1) > RMAT_ELMT(*rm, 0, 0)
&& RMAT_ELMT(*rm, 1, 1) > RMAT_ELMT(*rm, 2, 2);
ensures l_FloatQuat_of_RMat_1_max_t(rm) == l_Quat_of_FloatQuat(q);
behavior a22_max:
assumes trace(rm) <= 0
&& RMAT_ELMT(*rm, 2, 2) > RMAT_ELMT(*rm, 0, 0)
&& RMAT_ELMT(*rm, 2, 2) > RMAT_ELMT(*rm, 1, 1);
ensures l_FloatQuat_of_RMat_2_max_t(rm) == l_Quat_of_FloatQuat(q);
disjoint behaviors;
complete behaviors;
*/
extern void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm);
......
sw/airborne/math/pprz_algebra_float_convert_rmat_frama_c.h
View file @ d435994e
......@@ -26,12 +26,12 @@ extern "C" {
//@ type RealRMat = struct RealRMat_s;
//@ axiomatic Arbitrary_RealRMat { logic RealRMat empty;}
//@ axiomatic Arbitrary_RealRMat { logic RealRMat empty_rmat;}
#define REALRMAT(v00, v01, v02, \
v10, v11, v12, \
v20, v21, v22) \
{{{{{{{{{empty \
{{{{{{{{{empty_rmat \
\with .a00 = (float) (v00)} \with .a01 = (float) (v01)} \with .a02 = (float) (v02)}\
\with .a10 = (float) (v10)} \with .a11 = (float) (v11)} \with .a12 = (float) (v12)}\
\with .a20 = (float) (v20)} \with .a21 = (float) (v21)} \with .a22 = (float) (v22)}
......@@ -195,12 +195,133 @@ logic RealRMat l_RMat_of_FloatQuat_bis_2(struct FloatQuat *q) =
==> l_RMat_of_FloatQuat_bis_1(q) == l_RMat_of_FloatQuat_bis_2(q);
*/
/*******************************
* RealQuat definition
* *****************************/
#define SQR(a) (a*a)
/*@ ghost
struct RealQuat_s {float qi;
float qx;
float qy;
float qz;};
*/
//@ type RealQuat = struct RealQuat_s;
//@ axiomatic Arbitrary_RealQuat { logic RealQuat empty_quat;}
#define REALQUAT(vqi, \
vqx, \
vqy, \
vqz) \
{{{{empty_quat \
\with .qi = (float) (vqi)} \
\with .qx = (float) (vqx)} \
\with .qy = (float) (vqy)} \
\with .qz = (float) (vqz)}
/*@
logic RealQuat l_Quat_of_FloatQuat(struct FloatQuat *q) =
REALQUAT(q->qi, q->qx, q->qy, q->qz);
*/
/*******************************
* CONVERT RMAT TO QUAT
* *****************************/
/*
logic integer trace(RealRMat rmat) =
/*@
logic real trace(RealRMat rmat) =
rmat.a00 + rmat.a11 + rmat.a22;
*/
/*@
logic real trace(struct FloatRMat *rmat) =
trace(l_RMat_of_FloatRMat(rmat));
*/
// ************* Positive trace ******************
/*@
logic RealQuat l_FloatQuat_of_RMat_trace_pos(RealRMat rmat) =
\let T = 1.0 + trace(rmat);
\let S = 0.5 / \sqrt(T);
REALQUAT(0.25 / S,
(rmat.a21 - rmat.a12) * S,
(rmat.a02 - rmat.a20) * S,
(rmat.a10 - rmat.a01) * S);
*/
/*@
logic RealQuat l_FloatQuat_of_RMat_trace_pos(struct FloatRMat *rmat) =
l_FloatQuat_of_RMat_trace_pos(l_RMat_of_FloatRMat(rmat));
*/
/*@
logic RealQuat l_FloatQuat_of_RMat_trace_pos_t(struct FloatRMat *rmat) =
l_FloatQuat_of_RMat_trace_pos(transpose(l_RMat_of_FloatRMat(rmat)));
*/
// ************* A00 max ******************
/*@
logic RealQuat l_FloatQuat_of_RMat_0_max(RealRMat rmat) =
\let S = 2 * \sqrt( 1.0 + rmat.a00 - rmat.a11 - rmat.a22);
REALQUAT((rmat.a21 - rmat.a12) / S,
0.25 * S,
(rmat.a01 + rmat.a10) / S,
(rmat.a02 + rmat.a20) / S);
*/
/*@
logic RealQuat l_FloatQuat_of_RMat_0_max(struct FloatRMat *rmat) =
l_FloatQuat_of_RMat_0_max(l_RMat_of_FloatRMat(rmat));
*/
/*@
logic RealQuat l_FloatQuat_of_RMat_0_max_t(struct FloatRMat *rmat) =
l_FloatQuat_of_RMat_0_max(transpose(l_RMat_of_FloatRMat(rmat)));
*/
// ************* A11 max ******************
/*@
logic RealQuat l_FloatQuat_of_RMat_1_max(RealRMat rmat) =
\let S = 2 * \sqrt(1 - rmat.a00 + rmat.a11 - rmat.a22);
REALQUAT((rmat.a02 - rmat.a20) / S,
(rmat.a01 + rmat.a10) / S,
0.25 * S,
(rmat.a12 + rmat.a21) / S);
*/
/*@
logic RealQuat l_FloatQuat_of_RMat_1_max(struct FloatRMat *rmat) =
l_FloatQuat_of_RMat_1_max(l_RMat_of_FloatRMat(rmat));
*/
/*@
logic RealQuat l_FloatQuat_of_RMat_1_max_t(struct FloatRMat *rmat) =
l_FloatQuat_of_RMat_1_max(transpose(l_RMat_of_FloatRMat(rmat)));
*/
// ************* A22 max ******************
/*@
logic RealQuat l_FloatQuat_of_RMat_2_max(RealRMat rmat) =
\let S = 2 * \sqrt(1 - rmat.a00 - rmat.a11 + rmat.a22);
REALQUAT((rmat.a10 - rmat.a01) / S,
(rmat.a02 + rmat.a20) / S,
(rmat.a12 + rmat.a21) / S,
0.25 * S);
*/
/*@
logic RealQuat l_FloatQuat_of_RMat_2_max(struct FloatRMat *rmat) =
l_FloatQuat_of_RMat_2_max(l_RMat_of_FloatRMat(rmat));
*/
/*@
logic RealQuat l_FloatQuat_of_RMat_2_max_t(struct FloatRMat *rmat) =
l_FloatQuat_of_RMat_2_max(transpose(l_RMat_of_FloatRMat(rmat)));
*/
#ifdef __cplusplus
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment