[RSDK-732] orientation vector and rotation matrices #20
Conversation
src/spatialmath/utils.rs
Outdated
| OrientationVector{o_vector, theta} | ||
| } | ||
|
|
||
| pub fn from_quaternion(quat: &Quaternion<f64>) -> Self { |
There was a problem hiding this comment.
could you also implement the To and From traits?
There was a problem hiding this comment.
I can do it for the custom structs I created, but for quaternion and rotation matrix I would have to create wrappers. Is it fine to skip those two?
| @@ -204,7 +252,7 @@ mod tests { | |||
| let expected_ov = OrientationVector::new( | |||
| 0.0, -1.0, 0.0, 1.5707963267948966 | |||
| ); | |||
| let calc_ov = OrientationVector::from_quaternion(&quat); | |||
| let calc_ov: OrientationVector = quat.into(); | |||
There was a problem hiding this comment.
[minor] round-trip test might be good if it makes sense in this PR, rather than your next ticket.
There was a problem hiding this comment.
don't these two tests essentially provide the equivalent of a round-trip test? (the expected quaternion in one test is one of the input quaternions in the other and for the same case the opposite is true)
There was a problem hiding this comment.
Can you show me where in person? Approving to unblock you, but I'm not sure I'm seeing that round trip logic.
| /// but the canonical "right-hand rule" is used to select one consistently). Then, | ||
| /// a clockwise-rotation of theta is applied about the new-z axis | ||
| /// | ||
| /// It is highly recommended not to attempt any mathematics with the orientation |
There was a problem hiding this comment.
Euler angles should also have this warning
| /// theta (in radians). However, unlike an axis-angle, an orientation vector alters | ||
| /// the axes of the given frame of reference by rotating the z-axis to the | ||
| /// vector axis provided. The new x-axis is the vector that is both orthogonal to | ||
| /// the vector axis provided anc co-planar with both the old |
| let j = c[0]*s[1]*c[2] + s[0]*s[1]*s[2]; | ||
| let k = s[0]*c[1]*c[2] + c[0]*c[1]*s[2]; | ||
|
|
||
| Quaternion::new(real, i, j, k) |
There was a problem hiding this comment.
Not familiar with Rust, but would it be faster to put this math directly in the new() call and not initialize the variables?
There was a problem hiding this comment.
I don't think so, I think Rust ownership rules make both ways essentially equivalent
| // and must calculate roll based on the real rotation and yaw | ||
| if pitch_sin.abs() >= 1.0 { | ||
| pitch = (std::f64::consts::PI / 2.0).copysign(pitch_sin); | ||
| roll = (2.0 * norm_quat.i.atan2(norm_quat.w)) + yaw.copysign(pitch_sin); |
There was a problem hiding this comment.
Note: We should make this change in rdk as well.
this will be the last PR for the spatialmath library until Jan, at which point we might start importing it into the SDKs