def refPolarFromXY(x, y, tai):
"""Reference implementation of PVT.polarFromXY
"""
atPole = False
rArr = []
thetaArr = []
for testTAI in (tai, tai + DeltaT):
ap, r, theta = coordConv.polarFromXY(x.getPos(testTAI), y.getPos(testTAI))
rArr.append(r)
thetaArr.append(theta)
atPole = atPole or ap
rPVT = coordConv.PVT()
rPVT.pos = rArr[0]
rPVT.vel = (rArr[1] - rArr[0]) / DeltaT
rPVT.t = tai
thetaPVT = coordConv.PVT()
thetaPVT.pos = thetaArr[0]
thetaPVT.vel = coordConv.wrapCtr(thetaArr[1] - thetaArr[0]) / DeltaT
thetaPVT.t = tai
return atPole, rPVT, thetaPVT
def testPolarFromXY(self):
"""Test polarFromXY and xyFromPolar
"""
sqrt2 = math.sqrt(2.0)
for xy, predPol in (
((1, 0), (False, 1, 0)),
((-1, 0), (False, 1, 180)),
((0, 1), (False, 1, 90)),
((0, -1), (False, 1, -90)),
((1, 1), (False, sqrt2, 45)),
((1, -1), (False, sqrt2, -45)),
((-1, 1), (False, sqrt2, 135)),
((-1, -1), (False, sqrt2, -135)),
((-123.45, -123.45), (False, 123.45 * sqrt2, -135)),
((0, 0), (True, 0, 0)),
):
pol = coordConv.polarFromXY(*xy)
self.assertEqual(pol[0], predPol[0])
self.assertTrue(numpy.allclose(pol[1:], predPol[1:]))
compXY = coordConv.xyFromPolar(*pol[1:])
self.assertTrue(numpy.allclose(xy, compXY))
def testPolarFromXY(self):
"""Test polarFromXY and xyFromPolar
"""
for xPos, yPos, predAtPole in (
( 1, 0, False),
(-1, 0, False),
( 0, 1, False),
( 0, -1, False),
( 1, 1, False),
( 1, -1, False),
(-1, 1, False),
(-1, -1, False),
(-123.45, -123.45, False),
(0, 0, True),
):
for xVel in (-1, 0, 1):
for yVel in (-1, 0, 1):
x = coordConv.PVT(xPos, xVel, 10)
y = coordConv.PVT(yPos, yVel, 10)
r = coordConv.PVT()
theta = coordConv.PVT()
for endTime in (5, 10, 15):
atPole = coordConv.polarFromXY(r, theta, x, y, endTime)
refAtPole, refR, refTheta = refPolarFromXY(x, y, endTime)
self.assertEqual(atPole, refAtPole)
self.assertEqual(r.t, endTime)
self.assertEqual(refR.t, endTime)
self.assertEqual(theta.t, endTime)
self.assertEqual(refTheta.t, endTime)
self.assertTrue(numpy.allclose(
(r.pos, r.vel, theta.pos, theta.vel),
(refR.pos, refR.vel, refTheta.pos, refTheta.vel),
))
refX = coordConv.PVT()
refY = coordConv.PVT()
coordConv.xyFromPolar(refX, refY, r, theta, endTime)
self.assertTrue(numpy.allclose(
(x.pos, x.vel, y.pos, y.vel),
(refX.getPos(10), refX.vel, refY.getPos(10), refY.vel),
))
