c compute the path of a rod attached downward from a second
c rod held by two inverted pendulums.
        dimension xr(101), yr(101), xl(101), yl(101)
	dimension xlower0(101), ylower0(101)
	dimension xlower1(101), ylower1(101)
	dimension xlower2(101), ylower2(101)
	dimension xlower3(101), ylower3(101)
	logical limit
        pi = 4.*atan(1.0)
        dpr = 180./pi
        rpd = pi/180.
        print *, 'pi = ', pi
        print *, 'radians per degree = ', rpd
        print *, 'degrees per radian = ', dpr

c base of left inverted pendulum, x1, y1
        x1 = 0.
        y1 = 0.
c base of right inverted pendulum, x2, y2
        x2 = 30.
        y2 = 0.
c length of each inverted pendulum, pl
        pl = 15.
c length of horizontal attachment rod, rh
        rh = 10.
c length of attached vertical rod with mass at lower end, rv
        rv = 15.
c compute initial x-offset of top of pendulum from center of its base
        xoffi = (x2 - rh)/2.0
c compute initial, centered, height of horizontal rod
        hi = sqrt(pl*pl - xoffi**2)
        print *, 'Initial height of horizontal rod = ',
     *    hi
c write out the centered location of the top of right pendulum
	write(10, 3) x2-xoffi, hi
3	format(2f12.5)
c write out the centered location of the top of the left pendulum
	write(10, 3) xoffi, hi
c compute initial location of bottom of vertical rod
        xvi = x2/2.
        yvi = hi - rv
        print *, 'Initial location of mass, x, y =  ',
     *    xvi, yvi
c compute initial angle of right-hand pendulum, measured
c clockwise from the vertical.
        angi = dpr*asin(-xoffi/pl)
        print *, 'Initial angle of right-hand pendulum = ',
     *    angi, ' degrees.'
c compute an x,y array corresponding to the top of the right
c hand pendulum for a swing from angi minus dang degrees to
c angi + dang degrees.
        dangi = 4.

c draw circle
	ang = 0.
	do i = 1, 361
	  xfoo = x2 + pl*sin(ang*rpd)
          yfoo = pl*cos(ang*rpd)
          write(10, 3) xfoo, yfoo
	  ang = ang + 1.
	enddo

c compute 100 locations
        do i = 1, 101
          ang = angi - dangi + (i-1)*2.*dangi/100.
          xr(i) = x2 + pl*sin(ang*rpd)
          yr(i) = pl*cos(ang*rpd)
	  print *, ang, xr(i), yr(i)
        enddo
c for each of these 101 locations, compute the position of
c the other end of the horizontal rod.  The rod forms one
c circle and the left pendulum forms another.  The upper intersection
c of these circles is the position we're looking for.

	do i = 1, 101
          call circ_int(x1, y1, pl, xr(i), yr(i), rh,
     *    x3a, y3a, x3b, y3b, limit)
	  if(limit) then
	    ntot = i - 1
	    goto 20
	  endif
c for this geometry, the second point is the one we want to use (x3b, y3b)
	  if(i .eq. 50)
     *      write(10, 3) x3b, y3b, xr(50), yr(50)
c compute the position of the center of the horizontal rod
	  xl(i) = x3b
       	  yl(i) = y3b
	  xcent = (xr(i) + x3b)/2.
	  ycent = (yr(i) + y3b)/2.
c compute the position of the lower end of the vertical rod
c and two added locations along the rod
	  xlower0(i) = xcent 
	  ylower0(i) = ycent 
	  xlower1(i) = xcent + rv * (yr(i) - y3b)/(3.*rh)
	  ylower1(i) = ycent - rv * (xr(i) - x3b)/(3.*rh)  
	  xlower2(i) = xcent + 2.* rv * (yr(i) - y3b)/(3.*rh)
	  ylower2(i) = ycent - 2.* rv * (xr(i) - x3b)/(3.*rh)  
	  xlower3(i) = xcent + rv * (yr(i) - y3b)/rh
	  ylower3(i) = ycent - rv * (xr(i) - x3b)/rh  
	enddo
	ntot = 101
20	do i = 1, ntot
	  write(10, 3) xl(i), yl(i)
	enddo
	do i = 1, ntot
	  write(10, 3) xr(i), yr(i)
	enddo
	do i = 1, ntot
	  write(10, 3) xlower0(i), ylower0(i)
	enddo
	do i = 1, ntot
	  write(10, 3) xlower1(i), ylower1(i)
	enddo
	do i = 1, ntot
	  write(10, 3) xlower2(i), ylower2(i)
	enddo
	do i = 1, ntot
	  write(10, 3) xlower3(i), ylower3(i)
	enddo
        stop
        end
	subroutine circ_int(x0, y0, r0, x1, y1, r1,
     *    x3a, y3a, x3b, y3b, limit)
	logical limit
	limit = .false.
c distance between, d
	d = sqrt((x0-x1)**2 + (y0-y1)**2)
	print *, 'Distance between centers = ', d
c check for an intersection
	if(d .gt. r0 + r1) then
	  print *, 'Circles are too far apart.'
	  limit = .true.
	  return
	endif
	if(d .lt. abs(r0 - r1)) then
	  print *, 'Circles are too close together.'
          limit = .true.
	  return
        endif
c http://astronomy.swin.edu.au/~pbourke/geometry/2circle/
        a = (r0**2 - r1**2 + d**2)/(2*d)
        print *, 'Distance from center 0 to bisector = ', a

        h = sqrt(r0**2 - a**2)
        print *, 'Half lenght of bisector = ', h

        x2 = x0 + a*(x1 - x0)/d
        y2 = y0 + a*(y1 - y0)/d
        print *, 'Center of bisector = ', x2, y2

        x3a = x2 + h*(y1 - y0)/d
        y3a = y2 - h*(x1 - x0)/d

        x3b = x2 - h*(y1 - y0)/d
        y3b = y2 + h*(x1 - x0)/d

        return
        end