def sch_to_xyz(self, sch):
radcur = self.peg.getRadiusOfCurvature(
) # Get the radius of curvature at the peg point
ellipsoid = Ellipsoid(a=radcur, e2=0.0)
xyz = [0 for i in range(3)]
llh = [0 for i in range(3)]
llh[0] = math.degrees(sch[1] / radcur)
llh[1] = math.degrees(sch[0] / radcur)
llh[2] = sch[2]
schv = ellipsoid.llh_to_xyz(llh)
schvt = MM.matrixVectorProduct(self.M, schv)
for i in range(3):
xyz[i] = schvt[i] + self.r_ov[i]
return xyz
def xyz_to_sch(self, xyz):
radcur = self.peg.getRadiusOfCurvature(
) # Get the radius of curvature at the peg point
ellipsoid = Ellipsoid(a=radcur, e2=0.0)
schvt = [0 for i in range(3)]
rschv = [0 for i in range(3)]
for i in range(3):
schvt[i] = xyz[i] - self.r_ov[i]
schv = MM.matrixVectorProduct(self.invM, schvt)
llh = ellipsoid.xyz_to_llh(schv)
rschv[0] = radcur * math.radians(llh[1])
rschv[1] = radcur * math.radians(llh[0])
rschv[2] = llh[2]
return rschv
def testMatrixVectorProduct(self):
ans = [14, 32, 50]
mV = MM.matrixVectorProduct(self.M, self.V)
for i in range(3):
self.assertAlmostEquals(mV[i], ans[i], 5)
def localsch_to_xyz(self, sch):
xyz = MM.matrixVectorProduct(self.sch2xyz, sch)
return xyz
def xyz_to_localsch(self, xyz):
sch = MM.matrixVectorProduct(self.xyz2sch, xyz)
return sch
