diff --git a/problems/so3-synthetic.py b/problems/so3-synthetic.py
index 5a283a34dded7dac711930b670ffbfd7e6bd9bad..206e197dca2f62b9ba6c224500122652e585fc65 100644
--- a/problems/so3-synthetic.py
+++ b/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)