@@ -23,6 +23,7 @@ public class QuaternionToTwist : Combinator<Quaternion, double>
23
23
/// in the direction that the tether exits the headstage. Note that negating this vector will result in
24
24
/// negating the direction of twisting.
25
25
/// </remarks>
26
+ [ Category ( Definitions . ConfigurationCategory ) ]
26
27
[ TypeConverter ( typeof ( NumericRecordConverter ) ) ]
27
28
[ Description ( "The direction vector specifying the axis around which to calculate the twist." ) ]
28
29
public Vector3 RotationAxis { get ; set ; } = Vector3 . UnitZ ;
@@ -35,30 +36,31 @@ public class QuaternionToTwist : Combinator<Quaternion, double>
35
36
/// <returns>The sequence of twist values, in units of turns.</returns>
36
37
public override IObservable < double > Process ( IObservable < Quaternion > source )
37
38
{
39
+ var rotationAxis = RotationAxis ;
40
+
38
41
return Observable . Defer ( ( ) =>
39
42
{
40
- double ? previousTwist = default ;
43
+ double ? previousAngleAboutAxis = default ;
41
44
return source . Select ( rotation =>
42
45
{
43
46
// project rotation axis onto the direction axis
44
- var direction = RotationAxis ;
45
- var rotationAxis = new Vector3 ( rotation . X , rotation . Y , rotation . Z ) ;
46
- var dotProduct = Vector3 . Dot ( rotationAxis , direction ) ;
47
- var projection = dotProduct / Vector3 . Dot ( direction , direction ) * direction ;
48
- var twist = new Quaternion ( projection , rotation . W ) ;
49
- twist = Quaternion . Normalize ( twist ) ;
47
+ var vectorPart = new Vector3 ( rotation . X , rotation . Y , rotation . Z ) ;
48
+ var dotProduct = Vector3 . Dot ( vectorPart , rotationAxis ) ;
49
+ var projection = dotProduct / Vector3 . Dot ( rotationAxis , rotationAxis ) * rotationAxis ;
50
+ var rotationAboutAxis = new Quaternion ( projection , rotation . W ) ;
51
+ rotationAboutAxis = Quaternion . Normalize ( rotationAboutAxis ) ;
50
52
if ( dotProduct < 0 ) // account for angle-axis flipping
51
53
{
52
- twist = - twist ;
54
+ rotationAboutAxis = - rotationAboutAxis ;
53
55
}
54
56
55
57
// normalize twist feedback in units of turns
56
- var twistAngle = 2 * Math . Acos ( twist . W ) ;
57
- var feedback = previousTwist . HasValue
58
- ? ( twistAngle - previousTwist . GetValueOrDefault ( ) + 3 * Math . PI ) % ( 2 * Math . PI ) - Math . PI
58
+ var angleAboutAxis = 2 * Math . Acos ( rotationAboutAxis . W ) ;
59
+ var twist = previousAngleAboutAxis . HasValue
60
+ ? ( angleAboutAxis - previousAngleAboutAxis . GetValueOrDefault ( ) + 3 * Math . PI ) % ( 2 * Math . PI ) - Math . PI
59
61
: 0 ;
60
- previousTwist = twistAngle ;
61
- return - feedback / ( 2 * Math . PI ) ;
62
+ previousAngleAboutAxis = angleAboutAxis ;
63
+ return - twist / ( 2 * Math . PI ) ;
62
64
} ) ;
63
65
} ) ;
64
66
}
0 commit comments