def test_velocity(self):
        for body in self.conn.space_center.bodies.values():
            if body.orbit is None:
                continue

            # Check body velocity in body's reference frame
            v = body.velocity(body.reference_frame)
            self.assertClose((0,0,0), v)

            # Check body velocity in parent body's non-rotating reference frame
            v = body.velocity(body.orbit.body.non_rotating_reference_frame)
            if body.orbit.inclination == 0:
                self.assertClose(0, v[1])
            else:
                self.assertNotClose(0, v[1])
            self.assertClose(body.orbit.speed, norm(v))

            # Check body velocity in parent body's reference frame
            v = body.velocity(body.orbit.body.reference_frame)
            if body.orbit.inclination == 0:
                self.assertClose(0, v[1])
            else:
                self.assertNotClose(0, v[1])
            angular_velocity = body.orbit.body.angular_velocity(body.orbit.body.non_rotating_reference_frame)
            self.assertClose(0, angular_velocity[0])
            self.assertClose(0, angular_velocity[2])
            rotational_speed = dot((0,1,0), angular_velocity)
            position = list(body.position(body.orbit.body.reference_frame))
            position[1] = 0
            radius = norm(position)
            rotational_speed *= radius
            #TODO: large error
            self.assertClose(abs(rotational_speed + body.orbit.speed), norm(v), error=200)

    def test_transform_position_between_vessel_and_celestial_bodies(self):
        p0 = self.sc.transform_position((0,0,0), self.ref_vessel, self.ref_kerbin)
        p1 = self.sc.transform_position((0,0,0), self.ref_vessel, self.ref_sun)
        p2 = self.sc.transform_position((0,0,0), self.ref_kerbin, self.ref_sun)

        p3 = tuple(x-y for (x,y) in zip(p1,p2))
        #TODO: sometimes there is a large difference?!?! but only sometimes...
        self.assertClose(norm(p0), norm(p3), error=500)

 def check_speeds(self, flight):
     up_direction = vector(flight.up_direction)
     velocity = vector(flight.velocity)
     vertical_speed = dot(velocity, up_direction)
     horizontal_speed = norm(velocity) - vertical_speed
     self.assertClose(norm(velocity), flight.speed, error=1)
     self.assertClose(horizontal_speed, flight.horizontal_speed, error=1)
     self.assertClose(vertical_speed, flight.vertical_speed, error=1)

 def check_speeds(self, flight):
     """ Check flight.velocity agrees with flight.*_speed """
     up_direction = (0,1,0)
     velocity = vector(flight.velocity)
     vertical_speed = dot(velocity, up_direction)
     horizontal_speed = norm(velocity) - vertical_speed
     self.assertClose(norm(velocity), flight.speed, error=1)
     self.assertClose(horizontal_speed, flight.horizontal_speed, error=1)
     self.assertClose(vertical_speed, flight.vertical_speed, error=1)

    def test_transform_velocity_between_celestial_bodies(self):
        v1 = self.sc.transform_velocity((0,0,0), (0,0,0), self.ref_nr_mun, self.ref_nr_kerbin)
        v2 = self.sc.transform_velocity((0,0,0), (0,0,0), self.ref_nr_kerbin, self.ref_nr_mun)
        self.assertClose(self.mun.orbit.speed, norm(v1))
        self.assertClose(self.mun.orbit.speed, norm(v2))
        self.assertClose(v1, tuple(-x for x in v2))

        v1 = self.sc.transform_velocity((0,0,0), (0,0,0), self.ref_nr_kerbin, self.ref_nr_sun)
        v2 = self.sc.transform_velocity((0,0,0), (0,0,0), self.ref_nr_sun, self.ref_nr_kerbin)
        self.assertClose(self.kerbin.orbit.speed, norm(v1))
        self.assertClose(self.kerbin.orbit.speed, norm(v2))
        self.assertClose(v1, tuple(-x for x in v2))

 def check_radius_and_speed(self, obj, orbit):
     # Compute position from orbital elements
     pos = compute_position(obj, orbit.body.non_rotating_reference_frame)
     # Compute radius from position
     radius = norm(pos) * 1000000
     self.assertClose(radius, orbit.radius, error=1)
     # Compute speed from radius
     speed = math.sqrt(orbit.body.gravitational_parameter * ((2 / radius) - (1 / orbit.semi_major_axis)))
     self.assertClose(speed, orbit.speed, error=1)

    def test_angular_velocity(self):
        for body in self.conn.space_center.bodies.values():
            # Check body's angular velocity relative to itself is zero
            av = body.angular_velocity(body.reference_frame)
            self.assertClose((0,0,0), av)

            # Check body's angular velocity relative to it's own non-rotating reference frame
            av = body.angular_velocity(body.non_rotating_reference_frame)
            self.assertClose((0,-1,0), normalize(av))
            self.assertClose(body.rotational_speed, norm(av))

    def check_speed(self, flight, ref):
        up = normalize(vector(self.vessel.position(ref)) - vector(self.vessel.orbit.body.position(ref)))
        v = self.vessel.velocity(ref)

        speed = norm(v)
        vertical_speed = dot(v, up)
        horizontal_speed = speed - abs(vertical_speed)

        self.assertClose(speed, flight.speed, error=0.5)
        self.assertClose(vertical_speed, flight.vertical_speed, error=0.5)
        self.assertClose(horizontal_speed, flight.horizontal_speed, error=0.5)

    def test_position(self):
        for body in self.conn.space_center.bodies.values():

            # Check body position in body's reference frame
            p = body.position(body.reference_frame)
            self.assertClose((0,0,0), p)

            # Check body position in parent body's reference frame
            if body.orbit is not None:
                p = body.position(body.orbit.body.reference_frame)
                if body.orbit.inclination == 0:
                    self.assertClose(0, p[1])
                else:
                    self.assertNotClose(0, p[1])
                self.assertClose(body.orbit.radius, norm(p), error=10)

    def check_directions(self, flight):
        """ Check flight.direction against flight.heading and flight.pitch """
        direction       = vector(flight.direction)
        up_direction    = (1,0,0)
        north_direction = (0,1,0)
        self.assertClose(1, norm(direction))

        # Check vessel direction vector agrees with pitch angle
        pitch = 90 - rad2deg(math.acos(dot(up_direction, direction)))
        self.assertClose(flight.pitch, pitch, error=2)

        # Check vessel direction vector agrees with heading angle
        up_component = dot(direction, up_direction) * vector(up_direction)
        north_component = normalize(vector(direction) - up_component)
        self.assertCloseDegrees(flight.heading, rad2deg(math.acos(dot(north_component, north_direction))), error=1)

    def check_directions(self, flight, check_against_orbital=True):
        direction       = vector(flight.direction)
        up_direction    = vector(flight.up_direction)
        north_direction = vector(flight.north_direction)
        self.assertClose(1, norm(direction))
        self.assertClose(1, norm(up_direction))
        self.assertClose(1, norm(north_direction))
        self.assertClose(0, dot(up_direction, north_direction))

        # Check vessel direction vector agrees with pitch angle
        pitch = 90 - rad2deg(math.acos(dot(up_direction, direction)))
        self.assertClose(flight.pitch, pitch, error=2)

        # Check vessel direction vector agrees with heading angle
        up_component = dot(direction, up_direction) * vector(up_direction)
        north_component = vector(direction) - up_component
        north_component = north_component / norm(north_component)
        self.assertClose(flight.heading, rad2deg(math.acos(dot(north_component, north_direction))), error=1)

        if check_against_orbital == True:
            # Check vessel directions agree with orbital directions
            # (we are in a 0 degree inclined orbit, so they should do)
            self.assertClose(1, dot(up_direction, vector(flight.radial)))
            self.assertClose(1, dot(north_direction, vector(flight.normal)))

    def test_flight_orbit_body_non_rotating_reference_frame(self):
        ref = self.vessel.orbit.body.non_rotating_reference_frame
        flight = self.vessel.flight(ref)
        self.check_properties_not_affected_by_reference_frame(flight)

        speed = 2246.1
        self.assertClose(speed, norm(flight.velocity), error=0.5)
        position = self.vessel.position(ref)
        direction = vector(cross(normalize(position), (0,1,0)))
        velocity = direction * speed
        self.assertClose(velocity, flight.velocity, error=2)
        self.assertClose(speed, flight.speed, error=0.5)
        self.assertClose(speed, flight.horizontal_speed, error=0.5)
        self.assertClose(0, flight.vertical_speed, error=0.5)

        self.check_speeds(flight)
        self.check_orbital_vectors(flight)

    def test_pre_attached_ports(self):
        """ Test ports that were pre-attached in the VAB """
        bottom_port = filter(lambda p: p.part.parent.title == 'Clamp-O-Tron Docking Port', self.parts.docking_ports)[0]
        top_port = bottom_port.part.parent.docking_port
        launch_clamp = bottom_port.part.children[0].launch_clamp

        self.assertEquals(self.state.docked, top_port.state)
        self.assertEquals(self.state.docked, bottom_port.state)
        self.assertEquals(top_port.part, bottom_port.docked_part)
        self.assertEquals(bottom_port.part, top_port.docked_part)

        # Undock
        mass_before = self.vessel.mass
        undocked = top_port.undock()
        self.vessel = self.conn.space_center.active_vessel
        mass_after = self.vessel.mass

        self.assertEquals(bottom_port.docked_part, None)
        self.assertEquals(top_port.docked_part, None)

        # Undocking
        self.assertNotEqual(self.vessel, undocked)
        self.assertLess(mass_after, mass_before)
        self.assertClose(mass_after, mass_before - undocked.mass)
        self.assertEquals(self.state.undocking, top_port.state)
        self.assertEquals(self.state.undocking, bottom_port.state)
        self.assertEquals(bottom_port.docked_part, None)
        self.assertEquals(top_port.docked_part, None)

        # Drop the port
        launch_clamp.release()
        while top_port.state == self.state.undocking:
            pass

        # Undocked
        self.assertEquals(self.state.ready, top_port.state)
        self.assertEquals(self.state.ready, bottom_port.state)
        self.assertEquals(bottom_port.docked_part, None)
        self.assertEquals(top_port.docked_part, None)
        distance = norm(top_port.position(bottom_port.reference_frame))
        self.assertGreater(distance, top_port.reengage_distance)
        self.assertLess(distance, top_port.reengage_distance*1.5)

    def check(self, node, deltav):
        self.assertClose(deltav[0], node.prograde)
        self.assertClose(deltav[1], node.normal)
        self.assertClose(deltav[2], node.radial)
        self.assertClose(norm(deltav), node.delta_v)
        self.assertClose(norm(deltav), node.remaining_delta_v, 0.2)

        bv = node.burn_vector(node.reference_frame)
        self.assertClose(norm(deltav), norm(bv))
        self.assertClose((0,1,0), normalize(bv))

        orbital_bv = node.burn_vector(node.orbital_reference_frame)
        self.assertClose(norm(deltav), norm(orbital_bv))
        self.assertClose((-deltav[2],deltav[0],deltav[1]), orbital_bv)

        d = node.direction(node.reference_frame)
        self.assertClose((0,1,0), d)
        orbital_d = node.direction(node.orbital_reference_frame)
        self.assertClose(normalize((-deltav[2],deltav[0],deltav[1])), orbital_d)

 def check_orbital_vectors(self, flight):
     # Check orbital direction vectors
     prograde    = vector(flight.prograde)
     retrograde  = vector(flight.retrograde)
     normal      = vector(flight.normal)
     normal_neg  = vector(flight.normal_neg)
     radial      = vector(flight.radial)
     radial_neg  = vector(flight.radial_neg)
     self.assertClose(1, norm(prograde))
     self.assertClose(1, norm(retrograde))
     self.assertClose(1, norm(normal))
     self.assertClose(1, norm(normal_neg))
     self.assertClose(1, norm(radial))
     self.assertClose(1, norm(radial_neg))
     self.assertClose(prograde, [-x for x in retrograde], error=0.01)
     self.assertClose(radial, [-x for x in radial_neg], error=0.01)
     self.assertClose(normal, [-x for x in normal_neg], error=0.01)
     self.assertClose(0, dot(prograde, radial), error=0.01)
     self.assertClose(0, dot(prograde, normal), error=0.01)
     self.assertClose(0, dot(radial, normal), error=0.01)

 def check_orbital_vectors(self, flight):
     """ Check orbital direction vectors """
     prograde    = vector(flight.prograde)
     retrograde  = vector(flight.retrograde)
     normal      = vector(flight.normal)
     anti_normal = vector(flight.anti_normal)
     radial      = vector(flight.radial)
     anti_radial = vector(flight.anti_radial)
     self.assertClose(1, norm(prograde))
     self.assertClose(1, norm(retrograde))
     self.assertClose(1, norm(normal))
     self.assertClose(1, norm(anti_normal))
     self.assertClose(1, norm(radial))
     self.assertClose(1, norm(anti_radial))
     self.assertClose(prograde, [-x for x in retrograde], error=0.01)
     self.assertClose(radial, [-x for x in anti_radial], error=0.01)
     self.assertClose(normal, [-x for x in anti_normal], error=0.01)
     self.assertClose(0, dot(prograde, radial), error=0.01)
     self.assertClose(0, dot(prograde, normal), error=0.01)
     self.assertClose(0, dot(radial, normal), error=0.01)

    def test_transform_position_between_celestial_bodies(self):
        p = self.sc.transform_position((0,0,0), self.ref_kerbin, self.ref_mun)
        self.assertClose(self.mun.orbit.radius, norm(p))

        p = self.sc.transform_position((0,0,0), self.ref_sun, self.ref_kerbin)
        self.assertClose(self.kerbin.orbit.radius, norm(p))

 def check(self, node, v):
     self.assertEqual(v[0], node.prograde)
     self.assertEqual(v[1], node.normal)
     self.assertEqual(v[2], node.radial)
     self.assertEqual([0,0,norm(v)], vector(node.vector))
     self.assertEqual(norm(v), node.delta_v)

 def test_transform_velocity_with_rotational_velocity(self):
     d = 100000 + 600000
     v = self.sc.transform_velocity((d,0,0), (0,0,0), self.ref_kerbin, self.ref_nr_kerbin)
     self.assertClose(d * self.kerbin.rotational_speed, norm(v))

    def undock_and_dock(self, port1, port2):
        vessel = self.sc.active_vessel

        # Do it twice - once to undock pre-attached ports, once to undock ports docked in flight
        for i in range(2):

            # Kill rotation
            if i > 0:
                vessel.control.sas = True
                time.sleep(3)
                vessel.control.sas = False
                time.sleep(1)

            self.assertEquals(self.state.docked, port1.state)
            self.assertEquals(self.state.docked, port2.state)
            self.assertEquals(port1.part, port2.docked_part)
            self.assertEquals(port2.part, port1.docked_part)

            # Undock
            mass_before = vessel.mass
            undocked = port1.undock()
            vessel = self.sc.active_vessel
            self.assertNotEqual(vessel, undocked)
            self.assertEquals(self.state.undocking, port1.state)
            self.assertEquals(self.state.undocking, port2.state)
            self.assertEquals(None, port2.docked_part)
            self.assertEquals(None, port1.docked_part)
            mass_after = vessel.mass
            self.assertLess(mass_after, mass_before)
            self.assertClose(mass_after, mass_before - undocked.mass)

            # Move backwards to reengage distance
            vessel.control.rcs = True
            vessel.control.forward = -0.25
            time.sleep(0.5)
            vessel.control.forward = 0
            while port1.state == self.state.undocking:
                pass
            self.assertEquals(self.state.ready, port1.state)
            self.assertEquals(self.state.ready, port2.state)
            self.assertEquals(None, port2.docked_part)
            self.assertEquals(None, port1.docked_part)
            distance = norm(port1.position(port2.reference_frame))
            self.assertGreater(distance, port1.reengage_distance)
            self.assertLess(distance, port1.reengage_distance*1.05)
            time.sleep(0.5)

            # Check undocking when not docked
            self.assertEquals(None, port1.undock())
            self.assertEquals(None, port2.undock())

            # Move forward
            vessel.control.forward = 0.25
            time.sleep(1)
            vessel.control.forward = 0
            vessel.control.rcs = False
            while port1.state == self.state.ready:
                pass

            # Docking
            self.assertEquals(self.state.docking, port1.state)
            self.assertEquals(self.state.docking, port2.state)
            while port1.state == self.state.docking:
                pass

            # Docked
            self.assertEquals(self.state.docked, port1.state)
            self.assertEquals(self.state.docked, port2.state)
            self.assertEquals(port1.part, port2.docked_part)
            self.assertEquals(port2.part, port1.docked_part)

            # Get the new vessel
            vessel = self.sc.active_vessel