Mathematics Of 3D Motion Control - Logitech Magellan/SPACE MOUSE Classic Programmer's Manual

<cs 1,0>
The checksum is also
transmitted in two bytes, with
six significant bits per byte. The
checksum is calculated with the
following formula:
<cs> = <cs1> * 64 + <cs0>
For an error-free transmission,
the checksum is equal to the
sum of the transmitted data
bytes of all six inputs. The TSM
performs a check by calculating
this value (each data byte is
interpreted as an unsigned
integer):
<cs> = <tx1> + <tx0> +
<ty1> + <ty0> +
<tz1> + <tz0> +
<ra1> + <ra0> +
<rb1> + <rb0> +
<rc1> + <tc0>
As an example, suppose the TSM transmits
the following data packet:
dí╢¢qαÇ₧j₧║Zà▄B\r
The corresponding values are as follows.
Input Char. Nibbles (Hex.)
x A1,B6
í╢
y 9B,71
¢q
z E0,80
αÇ
a 9E,6A
₧j
b 9E,BA
₧║
c 5A,85
Zà
cs DC,42
▄B
cs = A1 + B6 + 9B + 71 + E0 + 80 +
9E + 6A + 9E + BA + 5A + 85
= 702 (Hex.)
= 1794 (Dec.)

Mathematics of 3D Motion Control

This appendix outlines the mathematics
necessary for describing arbitrary translational
and rotational motion of an object. A cube
serves as a good example for demonstrating
the steps involved in the computations.
Suppose the center of the cube is originally
aligned with the origin of the xyz-coordinate
system and its faces are parallel to the xy-, yz-
and xz-planes. If the cube has an edge length
of 2 units, its corners are given by the
following set of eight points:
Lower 6 Calc'd
Bits Dec. Input
(Hex.) Value
21,36 33,54 118
1B,3B 27,49 -271
20,00 32,0 0
1E,2A 30,42 -86
1E,3A 30,58 -70
1A,05 26,5 -379
1C,02 28,2 1794
P (1, 1, 1)
1 old
P (-1, -1, 1)
3 old
P (1, 1, -1)
5 old
P (-1, -1, -1)
7 old
Note that a physical unit of length is not
required.
One-Step Motion
If the cube is moved due to a translational or
rotational displacement of the Magellan/SPACE
MOUSE cap, eight new points must be
generated using the eight old points. To
accomplish this, the Magellan/SPACE MOUSE
sends the six values X, Y, Z, A, B and C.
For the cube's translational motion, the values
X, Y and Z have to be added to the original
coordinates of the corner points. Thus a new
point P is generated from an old point P
new
using the equation
P = P + T
new old XYZ
Shown explicitly, this formula consists of the
following three equations:
P = P + X
new X old X
P = P + Y
new Y old Y
P = P + Z
new Z old Z
For the cube's rotational motion, the values A,
B and C have to be incorporated into a 3x3
rotation matrix R.
R R
é
11 12
ê
R R
R =
ê
21 22
ê
R R
ë
31 32
The matrix elements are computed as follows:
R = (cos A)(cos B)
11
R = (sin A)(cos C) – (cos A)(sin B)(sin C)
12
R = (sin A)(sin C) + (cos A)(sin B)(cos C)
13
R = -(sin A)(cos B)
21
R = (cos A)(cos C) + (sin A)(sin B)(sin C)
22
R = (cos A)(sin C) – (sin A)(sin B)(cos C)
23
R = -(sin B)
31
R = -(cos B)(sin C)
32
R = (cos B)(cos C)
33
Using this rotation matrix, the effects of
rotation are calculated with the formula
P = [R](P )
new old
Shown explicitly, this formula consists of the
following three equations:
P = (R )(P
new X 11 old X
P = (R )(P
new Y 21 old X
P = (R )(P
new Z 31 old X
Combining the equations for translational and
rotational motion yields the equation
18
P (-1, 1, 1)
2 old
P (1, -1, 1)
4 old
P (-1, 1, -1)
6 old
P (1, -1, -1)
8 old
R
ù
13
ú
R
ú
23
ú
R
û
33
) + (R )(P ) + (R )(P
12 old Y 13
) + (R )(P ) + (R )(P
22 old Y 23
) + (R )(P ) + (R )(P
32 old Y 33
old
)
old Z
)
old Z
)
old Z