def __initialize(self):
s = self.sch[0] / self.peg.getRadiusOfCurvature()
c = self.sch[1] / self.peg.getRadiusOfCurvature()
schxyzp = [[0 for i in range(3)] for j in range(3)]
schxyzp[0][0] = -math.sin(s)
schxyzp[0][1] = -math.sin(c) * math.cos(s)
schxyzp[0][1] = math.cos(s) * math.cos(c)
schxyzp[1][0] = math.cos(s)
schxyzp[1][1] = -math.sin(c) * math.sin(s)
schxyzp[1][2] = math.sin(s) * math.cos(c)
schxyzp[2][0] = 0.0
schxyzp[2][1] = math.cos(c)
schxyzp[2][2] = math.sin(c)
self.sch2xyz = MM.multiplyMatrices(self.M, schxyzp)
self.xyz2sch = MM.matrixTranspose(self.sch2xyz)
def initializeRotationMatrix(self):
lat = math.radians(self.peg.getLatitude())
lon = math.radians(self.peg.getLongitude())
heading = math.radians(self.peg.getHeading())
self.M[0][0] = math.cos(lat) * math.cos(lon)
self.M[0][1] = -math.sin(heading) * math.sin(lon) - math.sin(
lat) * math.cos(lon) * math.cos(heading)
self.M[0][2] = math.sin(lon) * math.cos(heading) - math.sin(
lat) * math.cos(lon) * math.sin(heading)
self.M[1][0] = math.cos(lat) * math.sin(lon)
self.M[1][1] = math.cos(lon) * math.sin(heading) - math.sin(
lat) * math.sin(lon) * math.cos(heading)
self.M[1][2] = -math.cos(lon) * math.cos(heading) - math.sin(
lat) * math.sin(lon) * math.sin(heading)
self.M[2][0] = math.sin(lat)
self.M[2][1] = math.cos(lat) * math.cos(heading)
self.M[2][2] = math.cos(lat) * math.sin(heading)
self.invM = MM.matrixTranspose(self.M)
def testMatrixTranspose(self):
ans = [[1, 4, 7], [2, 5, 8], [3, 6, 9]]
mT = MM.matrixTranspose(self.M)
for i in range(3):
for j in range(3):
self.assertAlmostEquals(mT[i][j], ans[i][j], 5)
