帮忙解 cos(th1)*py-sin(th1)*px=d2 方程。。。未知数是th1。。。
理论解过程：
cos(th1)*py-sin(th1)*px=d2
设：px=r*cos(k)
py=r*sin(k)
r=(px^2+py^2)^0.5
k=arctan(py/px)
则方程化为：cos(th1)*sin(k)-sin(th1)*cos(k)=d2/r
利用和差公式，化为：sin(k-th1)=d2/r
因为，0<d2/r<1 , 0<k-th1<pi ,又
cos(k-th1)=+/-(1-(d2/r)^2)^0.5
故有
k-th1=arctan(d2/(+/-)(r^2-d2^2)^0.5)
th1=arctan(py/px)-arctan(d2/(+/-)(r^2-d2^2)^0.5)