# Streatchy IK

# Streatchy IK
# select ctrl, then root joint
from pymel.core import*
import math
def getPiv(s):
    select(s)
    setToolTo('Move')
    piv = manipMoveContext('Move', q=True, p=True,m=2)
    #piv = s.getTranslation()
    return piv
def getDist(p1,p2):
    dist = math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2 + (p1[2] - p2[2])**2)
    return dist
#sl = selected()
jnt1 = sl[1]
jnt2 = listRelatives(jnt1, c=1)[0]
ctrl = sl[0]

jnt1Pos = getPiv(jnt1)
jnt2Pos = getPiv(jnt2)
ctrlPos = getPiv(ctrl)

distanceNode = createNode("distanceDimShape")
dStart = spaceLocator(n=str(jnt1)+"_startPoint")
dEnd = spaceLocator(n=str(ctrl)+"_endPoint")

move(dStart, jnt1Pos)
pc = pointConstraint(ctrl, dEnd)
dStart.translate >> distanceNode.startPoint
dEnd.translate >> distanceNode.endPoint

mD = createNode("multiplyDivide")
setAttr(mD.operation, 2)
distanceNode.distance >> mD.input1X

conNode = createNode("condition")
setAttr(conNode.operation, 3)
mD.outputX >> conNode.colorIfTrueR
distanceNode.distance >> conNode.firstTerm

jD1 = getDist(jnt1Pos,jnt2Pos)
jD2 = getDist(jnt2Pos,ctrlPos)
setAttr(mD.input2X, jD1+jD2)
setAttr(conNode.secondTerm, jD1+jD2)
conNode.outColorR >> jnt1.scaleX
conNode.outColorR >> jnt2.scaleX