text: import vs
  import sys
  import math
  import sfmUtils
  from .dagutils import *
  from swigbullet import *
  
  unitsPerMeter = 53.3
  def vsToBtVector(vec):
  	return btVector3(vec.x/unitsPerMeter, vec.y/unitsPerMeter, vec.z/unitsPerMeter)
  #end
  def btToVsVector(vec):
  	return vs.Vector(vec.x()*unitsPerMeter, vec.y()*unitsPerMeter, vec.z()*unitsPerMeter)
  #end
  
  def vsToBtQuaternion(quat):
  	return btQuaternion(quat.x, quat.y, quat.z, quat.w)
  #end
  def btToVsQuaternion(quat):
  	return vs.Quaternion(quat.x(), quat.y(), quat.z(), quat.w())
  #end
  
  class World:
  	def __init__(self, gravity=btVector3(0,0,-10), airdensity=0.2):
  		self.broadphase = btDbvtBroadphase()
  		self.collisionConfiguration = btSoftBodyRigidBodyCollisionConfiguration()
  		self.dispatcher = btCollisionDispatcher(self.collisionConfiguration)
  		self.solver = btSequentialImpulseConstraintSolver()
  		self.softsolver = btDefaultSoftBodySolver()
  		self.world = btSoftRigidDynamicsWorld(
  			self.dispatcher, self.broadphase, 
  			self.solver, self.collisionConfiguration, self.softsolver)
  
  		self.world.setApplySpeculativeContactRestitution(True)
  		self.world.setGravity(gravity)
  
  		self.info = btSoftBodyWorldInfo()
  		self.info.air_density = airdensity
  		self.info.water_density = airdensity
  		self.info.m_broadphase = self.broadphase
  		self.info.m_dispatcher = self.dispatcher
  		self.info.m_gravity = gravity
  		self.info.m_sparsesdf.Initialize()
  
  		self.fixedStep = 1.0/180.0
  		self.elapsedTime = 0
  
  	def destroy(self):
  		del(self.world)
  		del(self.softsolver)
  		del(self.solver)
  		del(self.collisionConfiguration)
  		del(self.dispatcher)
  		del(self.broadphase)
  
  	def addRigidBody(self, rigidbody):
  		self.world.addRigidBody(rigidbody.body)
  
  	def addSoftBody(self, softbody):
  		self.world.addSoftBody(softbody.body)
  
  	def addConstraint(self, constraint):
  		self.world.addConstraint(constraint.cons, True)
  
  	def getFixedTimeStep(self):
  		return self.fixedStep
  
  	def stepWorld(self, dt):
  		maxsteps = int(1.0/self.fixedStep)
  		self.info.m_sparsesdf.GarbageCollect()
  		nsteps = self.world.stepSimulation(dt, maxsteps, self.fixedStep)
  		self.elapsedTime += dt
  
  		#sys.stderr.write("Elapsed time: "+str(self.elapsedTime)+" (dt="+str(dt)+"; substeps="+str(nsteps)+")\n")
  	#end
  
  	def getWorldInfo(self):
  		return self.info
  	#end
  #end
  
  class Rigidbody:
  	def __init__(self, data):
  		#TODO: convex hulls
  		if (data.shape == "box"):
  			self.shape = btBoxShape(vsToBtVector(data.boxsize))
  		elif (data.shape == "sphere"):
  			posdata = btVector3Array(1)
  			posdata[0] = btVector3(0,0,0)
  			radiusdata = floatArray(1)
  			radiusdata[0] = 1
  
  			self.shape = btMultiSphereShape(posdata, radiusdata, 1)
  			self.shape.setLocalScaling(vsToBtVector(data.boxsize))
  
  			del posdata
  			del radiusdata
  		#end
  
  		self.shape.setMargin(data.margin)
  
  		self.motion = btDefaultMotionState(btTransform(vsToBtQuaternion(data.quat), vsToBtVector(data.pos)))
  
  		self.inertia = btVector3(0,0,0)
  		self.mass = data.mass
  		self.shape.calculateLocalInertia(self.mass, self.inertia)
  
  		self.body = btRigidBody(self.mass, self.motion, self.shape, self.inertia)
  		if (self.mass == 0):
  			self.body.setCollisionFlags(self.body.getCollisionFlags() | btCollisionObject.CF_KINEMATIC_OBJECT)
  		#end
  
  		self.body.setActivationState(DISABLE_DEACTIVATION)
  		self.body.setRestitution(data.bounce)
  		self.body.setFriction(data.friction)
  		self.body.setDamping(data.lindamp, data.rotdamp)
  	#end
  
  	def setTransform(self, pos, quat):
  		self.motion.setWorldTransform(btTransform(vsToBtQuaternion(quat), vsToBtVector(pos)))
  	#end
  
  	def getTransform(self):
  		trans = btTransform()
  		self.motion.getWorldTransform(trans)
  		return btToVsVector(trans.getOrigin()), btToVsQuaternion(trans.getRotation())
  	#end
  
  	def addForce(self, pos, rot):
  		self.body.applyCentralForce(vsToBtVector(pos));
  		self.body.applyTorque(vsToBtVector(rot));
  	#end
  #end
  
  def degToRad(angle):
  	return angle*(math.pi/180.0)
  #end
  
  class Constraint:
  	def __init__(self, data, bodya, bodyb):
  		#data.bodya,b are strings
  		#let the sim script determine which btRigidBody corresponds to a and b
  		#and pass them as separate parameters
  		self.bodya = data.bodya
  		self.bodyb = data.bodyb
  
  		jointTransform = btTransform(vsToBtQuaternion(data.quat), vsToBtVector(data.pos))
  		transa = btTransform()
  		transb = btTransform()
  		bodya.motion.getWorldTransform(transa)
  		bodyb.motion.getWorldTransform(transb)
  		framea = transa.inverse() * jointTransform
  		frameb = transb.inverse() * jointTransform
  
  		if (data.constype == "point"):
  			self.cons = btPoint2PointConstraint(bodya.body, bodyb.body, framea.getOrigin(), frameb.getOrigin())
  		elif (data.constype == "cone"):
  			self.cons = btConeTwistConstraint(bodya.body, bodyb.body, framea, frameb)
  			self.cons.setLimit(degToRad(data.rotx), degToRad(data.roty), degToRad(data.twist))
  		#end
  	#end
  #end
  
  class Softbody:
  	def __init__(self, data, worldinfo):
  		nnodes = len(data.nodelist)
  		posdata = btVector3Array(nnodes)
  		massdata = floatArray(nnodes)
  
  		counter = 0
  		for node in data.nodelist:
  			posdata[counter] = vsToBtVector(node[2])
  			massdata[counter] = 20.0*node[1]/float(nnodes)
  			counter+=1
  		#end
  
  		self.body = btSoftBody(worldinfo, nnodes, posdata, massdata)
  		btSoftBodySetStretch(self.body, 1-data.stretch)
  		btSoftBodySetShear(self.body, 1-data.shear)
  
  		del posdata
  		del massdata
  
  		for link in data.linklist:
  			self.body.appendLink(link[0], link[1])
  		#end
  
  		for face in data.facelist:
  			self.body.appendTetra(face[0], face[1], face[2], face[3])
  		#end
  
  		bend = self.body.appendMaterial()
  		bend.m_kLST = 1 - data.bend
  		self.body.generateBendingConstraints(2, bend)
  
  		self.body.m_cfg.kDP = data.damp
  		self.body.m_cfg.kDF = data.friction
  		self.body.m_cfg.kSRHR_CL = 1.0
  		#self.body.m_cfg.piterations = 20
  		#self.body.m_cfg.citerations = 20
  		self.body.m_cfg.collisions = fCollision.CL_SS+fCollision.CL_RS+fCollision.CL_SELF;
  
  		self.body.randomizeConstraints()
  		self.body.setActivationState(DISABLE_DEACTIVATION)
  		self.body.getCollisionShape().setMargin(data.margin)
  		self.body.generateClusters(0)
  	#end
  
  	def setPosition(self, n, pos):
  		btSoftBodySetNodePosition(self.body, n, vsToBtVector(pos))
  	#end
  
  	def getPosition(self, n):
  		return btToVsVector(btSoftBodyGetNodePosition(self.body, n))
  	#end
  #end
  