@Thaumaturge Genuis man!!! stickList works like a charm. Added all the cone constraints no problem. Now I just need to figure out why they fly into the air. New code is now:
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from math import sin, cos, radians
import direct.directbase.DirectStart
from panda3d.core import Vec3, TransformState, Point3
from panda3d.bullet import BulletWorld, BulletPlaneShape, \
BulletRigidBodyNode, BulletCylinderShape, BulletDebugNode, ZUp, BulletConeTwistConstraint
from direct.showbase.DirectObject import DirectObject
def Rules(oldCharacter):
newCharacter = ""
if oldCharacter == 'F':
newCharacter = "B-F"
elif oldCharacter == 'B':
newCharacter = "F+B"
elif oldCharacter == '+':
newCharacter = ']B+F--B['
elif oldCharacter == '-':
newCharacter = '[F-B++F]'
elif oldCharacter == '[':
newCharacter = '^[FB+-]^'
elif oldCharacter == ']':
newCharacter = '.]BF-+[.'
else:
newCharacter = oldCharacter
return newCharacter
def processString(oldString):
newString = ""
for eachChar in oldString:
newString = newString + Rules(eachChar)
return ('FF' + newString)
def createLSystem(numofIters, startString):
endString = ""
for i in range(numofIters):
endString = processString(startString)
startString = endString
return endString
def toCoordinates(commandSequence, distance, degrees_xy, degrees_z):
angle = radians(0)
pitch = radians(0)
gps = np.array([0, 0, 0])
for character in commandSequence:
if character == "F":
try:
gps = np.vstack(
(gps, gps[(len(gps) - 1)] + [sin(radians(90) - angle)*distance, cos(radians(90) - angle)*distance, sin(pitch)*distance]))
except:
gps = np.vstack((gps, [distance, 0, 0]))
if character == "B":
try:
gps = np.vstack(
(gps, gps[(len(gps) - 1)] - [sin(radians(90)-angle)*distance, cos(radians(90)-angle)*distance, sin(pitch)*distance]))
except:
gps = np.vstack((gps, [-distance, 0, 0]))
if character == "+":
angle = angle + radians(degrees_xy)
if character == '-':
angle = angle - radians(degrees_xy)
if character == "^":
pitch = pitch + radians(degrees_z)
if character == '.':
pitch = pitch - radians(degrees_z)
if character == '[':
try:
savedCoor = np.vstack((savedCoor, (len(gps) - 1)))
except:
savedCoor = (len(gps) - 1)
if character == ']':
try:
loc_T = int(savedCoor[len(savedCoor) - 1])
loc_P = (len(gps) - 1)
gps = np.insert(gps, (loc_P), gps[loc_T], axis=0)
except:
pass
return gps.astype(int)
def drawLSystem(ax, vector1, vector2, time):
if time == 0:
ax.scatter(vector1[0], vector1[1], vector1[2], color='k', marker="*")
ax.scatter(vector2[0], vector2[1], vector2[2], color='b', marker="*")
ax.plot([vector1[0], vector2[0]], [vector1[1], vector2[1]],
[vector1[2], vector2[2]], color='r')
else:
ax.scatter(vector1[0], vector1[1], vector1[2], color='b', marker="*")
ax.scatter(vector2[0], vector2[1], vector2[2], color='b', marker="*")
ax.plot([vector1[0], vector2[0]], [vector1[1], vector2[1]],
[vector1[2], vector2[2]], color='r')
def toggleDebug():
if debugNP.isHidden():
debugNP.show()
else:
debugNP.hide()
def Simulate(coordinates):
base.cam.setPos(0, -1000, 1000)
base.cam.lookAt(0, 0, 0)
# Debug node
debugNode = BulletDebugNode('Debug')
debugNode.showWireframe(True)
debugNode.showConstraints(True)
debugNode.showBoundingBoxes(False)
debugNode.showNormals(False)
debugNP = render.attachNewNode(debugNode)
debugNP.show()
# World
world = BulletWorld()
world.setGravity(Vec3(0, 0, -9.81))
world.setDebugNode(debugNP.node())
# Plane
shape = BulletPlaneShape(Vec3(0, 0, 1), 1)
node = BulletRigidBodyNode('Ground')
node.addShape(shape)
ground = render.attachNewNode(node)
ground.setPos(0, 0, 0)
world.attachRigidBody(node)
stickList = []
for sticks in range(len(coordinates)-1):
length = np.linalg.norm(
coordinates[(sticks + 1)] - coordinates[sticks])
shape = BulletCylinderShape(0.1, length, ZUp)
node = BulletRigidBodyNode('legs' + str(sticks))
node.setMass(1)
node.addShape(shape)
cylinder = render.attachNewNode(node)
stickList.append(cylinder)
pos = coordinates[sticks]
cylinder.setPos(Vec3(pos[0], pos[1], pos[2]))
world.attachRigidBody(node)
print(stickList)
for lines in range(len(stickList) - 1):
frameA = TransformState.makePosHpr(Point3(0, 0, -5), Vec3(0, 0, -90))
frameB = TransformState.makePosHpr(Point3(0, 0, 5), Vec3(0, 0, -90))
swing1 = 60 # degrees
swing2 = 36 # degrees
twist = 120 # degrees
Stick1 = stickList[lines]
Stick2 = stickList[lines + 1]
cs = BulletConeTwistConstraint(
Stick1.node(), Stick2.node(), frameA, frameB)
cs.setDebugDrawSize(2.0)
cs.setLimit(swing1, swing2, twist)
world.attachConstraint(cs)
def update(task):
dt = globalClock.getDt()
world.doPhysics(dt)
return task.cont
o = DirectObject()
o.accept('f1', toggleDebug)
taskMgr.add(update, 'update')
base.run()
def main():
instance = createLSystem(2, "F")
# print("\n" + instance + "\n")
path = toCoordinates(instance, 10, 60, 90)
print(path)
Simulate(path)
main()
