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)