def test_structureSeed(): highLightDisProp = displayProperties([100,100,255],7) s = structureSeed(location(0.0,0.0,0.0),location(-math.sqrt(2.0),0.0,0.0))#Seed spand is always assumed to lie on the X-axis, expanding onto the Y axis leftHipFrontJ = s.jointA leftHipBackJ = s.jointB leftActJ = s.addJoint(leftHipFrontJ, 1.0, leftHipBackJ, math.sqrt(2), location(0.0,0.0,1.0), 'f', dispPropA='hi', dispPropB='hi',dispPropC='hi')#highLightDisProp) #2D joint, with a throw-away poitn for reference #leftFootJ = s.addJoint(leftHipFrontJ, 3.0, leftActJ) #Missing the last data asssumes a direction #The Joints moving to the middle #centerTopJ = s.addJoint(leftHipFrontJ,1.0, leftHipBackJ, math.sqrt(2.0), leftActJ, math.sqrt(2.0)) #centerBottomJ = s.addJoint(centerTopJ, 1.0,leftHipBackJ,math.sqrt(3.0),leftHipFrontJ,math.sqrt(2.0)) #Now spanning to the right foor #rightHipBackJ = s.addJoint(centerTopJ, math.sqrt(2.0),leftActJ, 2.0, centerBottomJ, math.sqrt(3.0)) #rightHipFrontJ = s.addJoint(centerTopJ, 1.0, rightHipBackJ, 1.0, centerBottomJ, math.sqrt(2.0)) #rightActJ = s.addJoint(rightHipFrontJ, 1.0, rightHipBackJ, math.sqrt(2), centerTopJ, math.sqrt(2.0)) #rightFootJ = s.addJoint(rightHipFrontJ, 3.0, rightActJ) return s
def internal_drawCenterOfMass(self,struct,crossSize=1.0,displayProps=displayProperties([255,255,255],5),debugPrint=False):#TODO:Make cross size intelligent #We assume the structure's compute locations are up to date m = struct.getCenterOfMass() if debugPrint: print "x:%f,y:%f,z:%f,m:%f"%(m.x,m.y,m.z,m.m) lineXN = m.copy() lineXN.add(location(-crossSize,0,0)) lineXP = m.copy() lineXP.add(location(crossSize,0,0)) lineYN = m.copy() lineYN.add(location(0,-crossSize,0)) lineYP = m.copy() lineYP.add(location(0,crossSize,0)) lineZN = m.copy() lineZN.add(location(0,0,-crossSize)) lineZP = m.copy() lineZP.add(location(0,0,crossSize)) if debugPrint: print "lineXN: %s"%lineXN.toString() print "lineXP: %s"%lineXP.toString() self.displayLines([[lineXN,lineXP,span(crossSize,displayProps=displayProps)],[lineYN,lineYP,span(crossSize,displayProps=displayProps)],[lineZN,lineZP,span(crossSize,displayProps=displayProps)]],clearDisp=False)
return sqrt(t) def sqrt(x): return math.sqrt(x) def sq(x): return math.pow(x,2.0) if __name__ == '__main__': #The simple struct totalLen = 30.0 mountLen = 10.0 refPt = [-1.0,0.0,0.0] subLen = totalLen/3.0 stretch = 1.001448 struct = mfStructure('lowerMount',[0,0,0],'upperMount',[0,mountLen,0]) struct.defaultDisplayProperties = displayProperties([255,100,255],2) struct.add2DJoint('lowerLoad','upperMount',stretch*diag(totalLen,mountLen),'lowerMount',totalLen,refPt,'f') struct.add2DJoint('upperLoad','upperMount',stretch*totalLen,'lowerLoad',mountLen,refPt,'f') struct.computeLocations() print "structure 1: upperLoad: %s"%struct.getJoint('upperLoad').toString() print "structure 1: lowerLoad: %s"%struct.getJoint('lowerLoad').toString() struct.defaultDisplayProperties = displayProperties([255,255,100],2) sectb = 3.0 struct.add2DJoint('lowerLoad1','upperMount',stretch*diag(totalLen/sectb,mountLen),'lowerMount',totalLen/sectb,refPt,'f') struct.add2DJoint('upperLoad1','upperMount',stretch*totalLen/sectb,'lowerLoad1',mountLen,refPt,'f') struct.add2DJoint('lowerLoad2','upperLoad1',stretch*diag(totalLen/sectb,mountLen),'lowerLoad1',totalLen/sectb,refPt,'f') struct.add2DJoint('upperLoad2','upperLoad1',stretch*totalLen/sectb,'lowerLoad2',mountLen,refPt,'f') struct.add2DJoint('lowerLoad3','upperLoad2',stretch*diag(totalLen/sectb,mountLen),'lowerLoad2',totalLen/sectb,refPt,'f') struct.add2DJoint('upperLoad3','upperLoad2',stretch*totalLen/sectb,'lowerLoad3',mountLen,refPt,'f')