Implement the derivative of the test function as well

So far: 2d only

problems/so3-synthetic.py

@@ -46,7 +46,132 @@ def matrixToQuaternion(m):
 
     return p
 
+# Takes a 3 x 3 object, returns a 4 x 3 x 3 object
+def derivativeOfMatrixToQuaternion(m):
+
+    result = [[[0,0,0],
+               [0,0,0],
+               [0,0,0]],
+              [[0,0,0],
+               [0,0,0],
+               [0,0,0]],
+              [[0,0,0],
+               [0,0,0],
+               [0,0,0]],
+              [[0,0,0],
+               [0,0,0],
+               [0,0,0]]]
+
+    p = [(1 + m[0][0] - m[1][1] - m[2][2]) / 4,
+         (1 - m[0][0] + m[1][1] - m[2][2]) / 4,
+         (1 - m[0][0] - m[1][1] + m[2][2]) / 4,
+         (1 + m[0][0] + m[1][1] + m[2][2]) / 4]
 
+    # avoid rounding problems
+    if (p[0] >= p[1] and p[0] >= p[2] and p[0] >= p[3]):
+
+        result[0] = [[1,0,0],[0,-1,0],[0,0,-1]]
+        for i in range(0,3):
+            for j in range(0,3):
+                result[0][i][j] *= 1.0/(8.0*sqrt(p[0]))
+
+        denom = 32 * pow(p[0],1.5)
+        offDiag = 1.0/(4*sqrt(p[0]))
+
+        result[1] = [ [-(m[0][1]+m[1][0]) / denom, offDiag,              0],
+                      [offDiag,                    (m[0][1]+m[1][0]) / denom, 0],
+                      [0,                          0,                         (m[0][1]+m[1][0]) / denom]]
+
+        result[2] = [ [-(m[0][2]+m[2][0]) / denom, 0,                        offDiag],
+                        [0,                         (m[0][2]+m[2][0]) / denom, 0],
+                        [offDiag,                    0,                       (m[0][2]+m[2][0]) / denom]]
+
+        result[3] = [ [-(m[2][1]-m[1][2]) / denom, 0,                         0],
+                      [0,                         (m[2][1]-m[1][2]) / denom, -offDiag],
+                      [0,                          offDiag,                   (m[2][1]-m[1][2]) / denom]]
+
+    elif (p[1] >= p[0] and p[1] >= p[2] and p[1] >= p[3]):
+
+        result[1] = [[-1,0,0],[0,1,0],[0,0,-1]]
+        for i in range(0,3):
+            for j in range(0,3):
+                result[1][i][j] *= 1.0/(8.0*sqrt(p[1]))
+
+        denom = 32 * pow(p[1],1.5)
+        offDiag = 1.0/(4*sqrt(p[1]))
+
+        result[0] = [ [(m[0][1]+m[1][0]) / denom, offDiag,                    0],
+                      [offDiag,                  -(m[0][1]+m[1][0]) / denom, 0],
+                      [0,                         0,                         (m[0][1]+m[1][0]) / denom]]
+
+        result[2] = [ [(m[1][2]+m[2][1]) / denom, 0           ,              0],
+                      [0,                        -(m[1][2]+m[2][1]) / denom, offDiag],
+                      [0,                         offDiag,                   (m[1][2]+m[2][1]) / denom]]
+
+        result[3] = [ [(m[0][2]-m[2][0]) / denom, 0,                         offDiag],
+                      [0,                        -(m[0][2]-m[2][0]) / denom, 0],
+                      [-offDiag,                  0,                         (m[0][2]-m[2][0]) / denom]]
+
+    elif (p[2] >= p[0] and p[2] >= p[1] and p[2] >= p[3]):
+
+        result[2] = [[-1,0,0],[0,-1,0],[0,0,1]]
+        for i in range(0,3):
+            for j in range(0,3):
+                result[2][i][j] *= 1.0/(8.0*sqrt(p[2]))
+
+        denom = 32 * pow(p[2],1.5)
+        offDiag = 1.0/(4*sqrt(p[2]))
+
+        result[0] = [ [(m[0][2]+m[2][0]) / denom, 0,                         offDiag],
+                      [0,                        (m[0][2]+m[2][0]) / denom, 0],
+                      [offDiag,                   0,                         -(m[0][2]+m[2][0]) / denom]]
+
+        result[1] = [ [(m[1][2]+m[2][1]) / denom, 0           ,              0],
+                      [0,                        (m[1][2]+m[2][1]) / denom,  offDiag],
+                      [0,                         offDiag,                 -(m[1][2]+m[2][1]) / denom]]
+
+        result[3] = [ [(m[1][0]-m[0][1]) / denom, -offDiag,                  0],
+                      [offDiag,                   (m[1][0]-m[0][1]) / denom, 0],
+                      [0,                          0,                        -(m[1][0]-m[0][1]) / denom]]
+
+    else:
+
+        result[3] = [[1,0,0],[0,1,0],[0,0,1]]
+        for i in range(0,3):
+            for j in range(0,3):
+                result[3][i][j] *= 1.0/(8.0*sqrt(p[3]))
+
+        denom = 32 * pow(p[3],1.5)
+        offDiag = 1.0/(4*sqrt(p[3]))
+
+        result[0] = [ [-(m[2][1]-m[1][2]) / denom, 0,                         0],
+                      [0,                         -(m[2][1]-m[1][2]) / denom, -offDiag],
+                      [0,                         offDiag,                   -(m[2][1]-m[1][2]) / denom]]
+
+        result[1] = [ [-(m[0][2]-m[2][0]) / denom, 0,                         offDiag],
+                      [0,                         -(m[0][2]-m[2][0]) / denom, 0],
+                      [-offDiag,                  0,                         -(m[0][2]-m[2][0]) / denom]]
+
+        result[2] = [ [-(m[1][0]-m[0][1]) / denom, -offDiag,                  0],
+                      [offDiag,                   -(m[1][0]-m[0][1]) / denom, 0],
+                      [0,                         0,                         -(m[1][0]-m[0][1]) / denom]]
+
+    return result
+
+
+
+def mult(m1,m2):
+    m = [[0,0,0],
+         [0,0,0],
+         [0,0,0]]
+
+    # Matrix-matrix multiplication by hand -- for some reason I cannot get numpy to work
+    for i in range(0,3):
+        for j in range(0,3):
+            for k in range(0,3):
+                m[i][j] = m[i][j] + m1[i][k] * m2[k][j]
+
+    return m
 
 def f(x):
 
@@ -61,20 +186,48 @@ def f(x):
                  [sin(beta),  cos(beta), 0],
                  [0, 0, 1]]
 
-    m = [[0,0,0],
-         [0,0,0],
-         [0,0,0]]
-
-    # Matrix-matrix multiplication by hand -- for some reason I cannot get numpy to work
-    for i in range(0,3):
-        for j in range(0,3):
-            for k in range(0,3):
-                m[i][j] = m[i][j] + rotationX[i][k] * rotationZ[k][j]
+    m = mult(rotationX,rotationZ)
 
     return matrixToQuaternion(m)
 
-# Should never be called
 def df(x):
-    return [[0,0],[0,0],[0,0],[0,0]]
+
+    alpha = 2*math.pi*x[0]
+    beta  = 2*math.pi*x[1]
+
+    rotationX = [[1, 0, 0],
+                 [0, cos(alpha), -sin(alpha)],
+                 [0, sin(alpha),  cos(alpha)]]
+
+    derRotX   = [[0, 0, 0],
+                 [0, -2*math.pi*sin(alpha), -2*math.pi*cos(alpha)],
+                 [0,  2*math.pi*cos(alpha), -2*math.pi*sin(alpha)]]
+
+    rotationZ = [[cos(beta), -sin(beta), 0],
+                 [sin(beta),  cos(beta), 0],
+                 [0, 0, 1]]
+
+    derRotZ   = [[-2*math.pi*sin(beta), -2*math.pi*cos(beta), 0],
+                 [ 2*math.pi*cos(beta), -2*math.pi*sin(beta), 0],
+                 [0, 0, 0]]
+
+    # The function value as matrix
+    # CAREFUL! We are reimplementing method 'f' here -- the two implementations need to match!
+    value = mult(rotationX,rotationZ)
+
+    derX0 = mult(derRotX,rotationZ)
+    derX1 = mult(rotationX,derRotZ)
+
+    derOfMatrixToQuaternion = derivativeOfMatrixToQuaternion(value)
+
+    result = [[0,0],[0,0],[0,0],[0,0]]
+
+    for i in range(0,4):
+        for j in range(0,3):
+            for k in range(0,3):
+                result[i][0] += derOfMatrixToQuaternion[i][j][k] * derX0[j][k]
+                result[i][1] += derOfMatrixToQuaternion[i][j][k] * derX1[j][k]
+
+    return result
 
 fdf = (f, df)