def createCapsule(self, mass=100, length=1, radius=1, pos=(0, 0, 0)):
"""Creates a capsule body and corresponding geom."""
body = ode.Body(self.world)
m = ode.Mass()
m.setCappedCylinderTotal(mass, 3, radius, length)
body.setMass(m)
# set parameters for drawing the body
body.shape = "capsule"
body.length = length
body.radius = radius
# create a capsule geom for collision detection
geom = ode.GeomCCylinder(self.space, radius, length)
geom.setBody(body)
# create a reference to the body geom in case the user does not want to keep track of it.
body.geom = geom
body.color = (128, 0, 0, 255)
# This is the constant offset for aligning with the physics element
# This is a 4x4 opengl style matrix, expressing an offset and a rotation
offset = (0, 0, 0)
rot = calcRotMatrix((0, 0, 1), 0)
body.glObjOffset = makeOpenGLMatrix(rot, offset, (1.0, 1.0, 1.0))
body.setPosition(pos)
self.bodies.append(body)
return body, geom
def addBody(self,
p1,
p2,
radius,
mass=None,
upVec=(0, 0, 1),
collide=True):
"""
Adds a capsule body between joint positions p1 and p2 and with given
radius to the ragdoll.
upVec is the vector pointing up. This resolves the ambiguity of
orienting a rotationally symetrical object between 2 points.
TODO: If p1->p2 || upVec, this will crash
"""
p1 = add3(p1, self.offset)
p2 = add3(p2, self.offset)
# cylinder length not including endcaps, make capsules overlap by half
# radius at joints
cyllen = dist3(p1, p2) - radius
body = ode.Body(self.sim.world)
body.color = (128, 128, 40, 255)
m = ode.Mass()
if mass is None:
m.setCappedCylinder(self.density, 3, radius, cyllen)
else:
m.setCappedCylinderTotal(mass, 3, radius, cyllen)
body.setMass(m)
# set parameters for drawing the body
body.shape = "capsule"
body.length = cyllen
body.radius = radius
# create a capsule geom for collision detection
if collide:
geom = ode.GeomCCylinder(self.sim.space, radius, cyllen)
geom.setBody(body)
self.geoms.append(geom)
# define body rotation automatically from body axis
za = norm3(sub3(p2, p1))
# if (abs(dot3(za, (1.0, 0.0, 0.0))) < 0.7): xa = (1.0, 0.0, 0.0)
# else: xa = (0.0, 1.0, 0.0)
xa = upVec
ya = cross(za, xa)
xa = norm3(cross(ya, za))
ya = cross(za, xa)
rot = (xa[0], ya[0], za[0], xa[1], ya[1], za[1], xa[2], ya[2], za[2])
body.setPosition(mul3(add3(p1, p2), 0.5))
body.setRotation(rot)
self.bodies.append(body)
self.totalMass += body.getMass().mass
return body
def create_capsule(self,key,density,length,radius,pos,base=0,rot=0,R=0.):
"""Creates a capsule body and corresponding geom.
Arguments:
key : number id to assign to the capsule
density : density of the given body
length : length of the capsule
radius : radius of the capsule
pos : position of the center of the capsule (x,y,z list)
base : place new object at negative end of base object
"""
# Auto label the joint key or not.
key = len(self.bodies) if key == -1 else key
# create capsule body (aligned along the z-axis so that it matches the
# GeomCCylinder created below, which is aligned along the z-axis by
# default)
body = ode.Body(self.world)
M = ode.Mass()
M.setCapsule(density, 3, radius, length)
body.setMass(M)
# create a capsule geom for collision detection
geom = ode.GeomCCylinder(self.space, radius, length)
geom.setBody(body)
# set the position of the capsule
body.setPosition((pos[0],pos[1],pos[2]))
# set parameters for drawing the body
body.shape = "capsule"
body.length = length
body.radius = radius
# set the rotation of the capsule
if(rot):
body.setRotation(self.form_rotation(rot))
# set the rotation of the capsule directly
if R:
body.setRotation(R)
self.bodies[key] = body
self.geoms[key] = geom
if(base):
Placement.place_object(self.bodies[base],body)
if(self.fluid_dynamics):
self.create_surfaces(key,1.)
return key
def create_cylinder(world, space, density, radius, height):
body = ode.Body(world)
M = ode.Mass()
M.setCappedCylinder(density, direction=1, r=radius, h=height)
body.setMass(M)
# Set parameters for drawing the body
body.shape = "ccylinder"
body.radius = radius
body.height = height
# Create a CCylinder geom for collision detection
geom = ode.GeomCCylinder(space, radius, height)
geom.setBody(body)
return body, geom
def addBody(self, p1, p2, radius):
"""
adds a capsule body between joint posotions p1 and p2 with given
radius to the ragdoll.
"""
p1 = add3(p1, self.offset)
p2 = add3(p2, self.offset)
# cylinder length not including endcaps, make capsules overlap by half
# radius at joints
cyllen = dist3(p1, p2) - radius
body = ode.Body(self.world)
m = ode.Mass()
m.setCappedCylinder(self.density, 3, radius, cyllen)
body.setMass(m)
# set parameters for drawing the body
body.shape = "capsule"
body.length = cyllen
body.radius = radius
# create a capsule geom for collision detection
geom = ode.GeomCCylinder(self.space, radius, cyllen)
geom.setBody(body)
# define body rotation automatically from body axis
za = norm3(sub3(p2, p1))
if (abs(dot3(za, (1.0, 0.0, 0.0))) < 0.7): xa = (1.0, 0.0, 0.0)
else: xa = (0.0, 1.0, 0.0)
ya = cross(za, xa)
xa = norm3(cross(ya, za))
ya = cross(za, xa)
rot = (xa[0], ya[0], za[0], xa[1], ya[1], za[1], xa[2], ya[2], za[2])
body.setPosition(mul3(add3(p1, p2), 0.5))
body.setRotation(rot)
self.bodies.append(body)
self.geoms.append(geom)
self.totalMass += body.getMass().mass
return body
def createCapsule(world, space, density, length, radius):
"""Creates a capsule body and corresponding geom."""
# create capsule body (aligned along the z-axis so that it matches the
# GeomCCylinder created below, which is aligned along the z-axis by
# default)
body = ode.Body(world)
M = ode.Mass()
M.setCappedCylinder(density, 3, radius, length)
body.setMass(M)
# set parameters for drawing the body
body.shape = "capsule"
body.length = length
body.radius = radius
# create a capsule geom for collision detection
geom = ode.GeomCCylinder(space, radius, length)
geom.setBody(body)
return body, geom
def addSpringyCappedCylinder(self,
p1,
p2,
radius,
n_members=2,
k_bend=1e5,
k_torsion=1e5):
"""
Adds a capsule body between joint positions p1 and p2 and with given
radius to the ragdoll.
Along the axis of the capsule distribute n_joints ball joints
With spring constants given by k_bend and k_torsion
TODO: FUNCTION NOT FINISHED Wed 14 Dec 2011 06:06:24 PM EST
"""
p_start = p1 = add3(p1, self.offset)
p_end = add3(p2, self.offset)
iteration_jump = div3(sub3(p_end, p_start), n_members)
p2 = add3(p1, iteration_jump)
for i in range(n_members):
# cylinder length not including endcaps, make capsules overlap by half
# radius at joints
cyllen = dist3(p1, p2) - radius
body = ode.Body(self.sim.world)
m = ode.Mass()
m.setCappedCylinder(self.density, 3, radius, cyllen)
body.setMass(m)
# set parameters for drawing the body
body.shape = "capsule"
body.length = cyllen
body.radius = radius
# create a capsule geom for collision detection
geom = ode.GeomCCylinder(self.sim.space, radius, cyllen)
geom.setBody(body)
# define body rotation automatically from body axis
za = norm3(sub3(p2, p1))
if (abs(dot3(za, (1.0, 0.0, 0.0))) < 0.7):
xa = (1.0, 0.0, 0.0)
else:
xa = (0.0, 1.0, 0.0)
ya = cross(za, xa)
xa = norm3(cross(ya, za))
ya = cross(za, xa)
rot = (xa[0], ya[0], za[0], xa[1], ya[1], za[1], xa[2], ya[2],
za[2])
body.setPosition(mul3(add3(p1, p2), 0.5))
body.setRotation(rot)
self.bodies.append(body)
self.geoms.append(geom)
self.totalMass += body.getMass().mass
if i != n_members:
p1 = p2
p2 = add3(p1, iteration_jump)
return body
def addBP(self, bp, parent=None, joint_end=None):
"""Recursively add BodyParts to the simulation.
bp -- current BodyPart to process
parent -- parent BP to add to
joint_end -- which end of the parent ccylinder we should add to"""
log.debug('addBP')
body = ode.Body(self.world)
mass = ode.Mass()
if not parent:
# if this is the root we have an absolute length,
# root scale is relative to midpoint of min..max
bp.length = bp.scale * (
bpg.BP_MIN_LENGTH +
(bpg.BP_MAX_LENGTH - bpg.BP_MIN_LENGTH) / 2)
bp.isRoot = 1
else:
# otherwise child scale is relative to parent
bp.length = parent.length * bp.scale
bp.isRoot = 0
# limit the bp length
bp.length = min(bp.length, bpg.BP_MAX_LENGTH)
# mass along x axis, with length without caps==bp.length
# arg 3 means aligned along z-axis - must be same in renderer and Geoms
mass.setCappedCylinder(CYLINDER_DENSITY, 3, CYLINDER_RADIUS, bp.length)
# attach mass to body
body.setMass(mass)
# create Geom
# aligned along z-axis by default!!
geom = ode.GeomCCylinder(self.space, CYLINDER_RADIUS, bp.length)
self.geoms.append(geom)
self.geom_contact[geom] = 0
# remember parent for collison detection
if not parent:
geom.parent = None
else:
geom.parent = parent.geom
# attach geom to body
geom.setBody(body)
log.debug('created CappedCylinder(radius=%f, len=%f)', CYLINDER_RADIUS,
bp.length)
# assert(not in a loop)
assert not hasattr(bp, 'geom')
# ref geom from bodypart (used above to find parent geom)
bp.geom = geom
# set rotation
(radians, v) = bp.rotation
log.debug('radians,v = %f,%s', radians, str(v))
q = quat(radians, vec3(v))
rotmat = q.toMat3()
if parent:
# rotate relative to parent
p_r = mat3(parent.geom.getRotation()) # joint_end *
log.debug('parent rotation = %s', str(p_r))
rotmat = p_r * rotmat
geom.setRotation(rotmat.toList(rowmajor=1))
log.debug('r=%s', str(rotmat))
geom_axis = rotmat * vec3(0, 0, 1)
log.debug('set geom axis to %s', str(geom_axis))
(x, y, z) = geom.getBody().getRelPointPos((0, 0, bp.length / 2.0))
log.debug('real position of joint is %f,%f,%f', x, y, z)
# set position
if not parent:
# root - initially located at 0,0,0
# (once the model is constructed we translate it until all
# bodies have z>0)
geom.setPosition((0, 0, 0))
log.debug('set root geom x,y,z = 0,0,0')
else:
# child - located relative to the parent. from the
# parents position move along their axis of orientation to
# the joint position, then pick a random angle within the
# joint limits, move along that vector by half the length
# of the child cylinder, and we have the position of the
# child.
# vector from parent xyz is half of parent length along
# +/- x axis rotated by r
p_v = vec3(parent.geom.getPosition())
p_r = mat3(parent.geom.getRotation())
p_hl = parent.geom.getParams()[1] / 2.0 # half len of par
j_v = p_v + p_r * vec3(0, 0, p_hl * joint_end) # joint vector
# rotation is relative to parent
c_v = j_v + rotmat * vec3(0, 0, bp.length / 2.0)
geom.setPosition(tuple(c_v))
log.debug('set geom x,y,z = %f,%f,%f', c_v[0], c_v[1], c_v[2])
jointclass = {
'hinge': ode.HingeJoint,
'universal': ode.UniversalJoint,
'ball': ode.BallJoint
}
j = jointclass[bp.joint](self.world)
self.joints.append(j)
# attach bodies to joint
j.attach(parent.geom.getBody(), body)
# set joint position
j.setAnchor(j_v)
geom.parent_joint = j
# create motor and attach to this geom
motor = Motor(self.world, None)
motor.setJoint(j)
self.joints.append(motor)
bp.motor = motor
geom.motor = motor
motor.attach(parent.geom.getBody(), body)
motor.setMode(ode.AMotorEuler)
if bp.joint == 'hinge':
# we have 3 points - parent body, joint, child body
# find the normal to these points
# (hinge only has 1 valid axis!)
try:
axis1 = ((j_v - p_v).cross(j_v - c_v)).normalize()
except ZeroDivisionError:
v = (j_v - p_v).cross(j_v - c_v)
v.z = 1**-10
axis1 = v.normalize()
log.debug('setting hinge joint axis to %s', axis1)
log.debug('hinge axis = %s', j.getAxis())
axis_inv = rotmat.inverse() * axis1
axis2 = vec3((0, 0, 1)).cross(axis_inv)
log.debug('hinge axis2 = %s', axis2)
j.setAxis(tuple(axis1))
# some anomaly here.. if we change the order of axis2 and axis1,
# it should make no difference. instead there appears to be an
# instability when the angle switches from -pi to +pi
# so.. use parameter3 to control the hinge
# (maybe this is only a problem in the test case with perfect axis alignment?)
motor.setAxes(axis2, rotmat * axis1)
elif bp.joint == 'universal':
# bp.axis1/2 is relative to bp rotation, so rotate axes
axis1 = rotmat * vec3(bp.axis1)
axis2 = rotmat * vec3((0, 0, 1)).cross(vec3(bp.axis1))
j.setAxis1(tuple(axis1))
j.setAxis2(tuple(axis2))
motor.setAxes(axis1, axis2)
elif bp.joint == 'ball':
axis1 = rotmat * vec3(bp.axis1)
axis2 = rotmat * vec3((0, 0, 1)).cross(vec3(bp.axis1))
motor.setAxes(axis1, axis2)
log.debug('created joint with parent at %f,%f,%f', j_v[0], j_v[1],
j_v[2])
# recurse on children
geom.child_joint_ends = set([e.joint_end for e in bp.edges])
geom.parent_joint_end = joint_end
if joint_end == None:
# root
if -1 in geom.child_joint_ends:
geom.left = 'internal'
else:
geom.left = 'external'
if 1 in geom.child_joint_ends:
geom.right = 'internal'
else:
geom.right = 'external'
else:
# not root
geom.left = 'internal'
if 1 in geom.child_joint_ends:
geom.right = 'internal'
else:
geom.right = 'external'
for e in bp.edges:
self.addBP(e.child, bp, e.joint_end)
def _createGeom(self, geom, M):
"""Create an ODE geom object for the given cgkit geom.
Create an ODE geom of the appropriate type (depening on \a geom)
and apply the transformation \a M to it (currently, M may only
contain a translation and a rotation, otherwise a warning is
printed).
The returned geom is not assigned to a Body yet.
\param geom (\c GeomObject) cgkit geom object
\param M (\c mat4) Transformation that's applied to the geom
\return ODE geom
"""
# Create the raw geom object that's defined in the local
# coordinate system L of the world object this geom belongs to.
# Sphere geom?
if isinstance(geom, SphereGeom):
odegeom = ode.GeomSphere(None, geom.radius)
# CCylinder geom?
elif isinstance(geom, CCylinderGeom):
odegeom = ode.GeomCCylinder(None, geom.radius, geom.length)
# Box geom?
elif isinstance(geom, BoxGeom):
odegeom = ode.GeomBox(None, (geom.lx, geom.ly, geom.lz))
# TriMesh geom?
elif isinstance(geom, TriMeshGeom):
verts = []
for i in range(geom.verts.size()):
verts.append(geom.verts.getValue(i))
# print "V",i,verts[i]
faces = []
for i in range(geom.faces.size()):
f = geom.faces.getValue(i)
faces.append(f)
# print "F",i,faces[i]
ia, ib, ic = faces[i]
a = verts[ia]
b = verts[ib]
c = verts[ic]
sa = ((b - a).cross(c - a)).length()
# if sa<0.0001:
# print "*****KLEIN*****",sa
tmd = ode.TriMeshData()
tmd.build(verts, faces)
odegeom = ode.GeomTriMesh(tmd, None)
# Plane geom?
elif isinstance(geom, PlaneGeom):
L = obj.localTransform()
n = L * vec3(0, 0, 1) - L * vec3(0)
n = n.normalize()
d = n * obj.pos
odegeom = ode.GeomPlane(space, n, d)
# Unknown geometry
else:
raise ValueError, 'WARNING: ODEDynamics: Cannot determine collision geometry of object "%s".' % geom.name
# print 'WARNING: ODEDynamics: Cannot determine collision geometry of object "%s". Using bounding box instead.'%obj.name
# bmin, bmax = obj.boundingBox().getBounds()
# s = bmax-bmin
# odegeom = ode.GeomBox(None, s)
# pos,rot,scale = P.inverse().decompose()
# odegeom.setPosition(pos + 0.5*(bmax+bmin))
# odegeomtransform = ode.GeomTransform(space)
# odegeomtransform.setGeom(odegeom)
# return odegeomtransform
# Displace the geom by M
pos, rot, scale = M.decompose()
if scale != vec3(1, 1, 1):
print 'WARNING: ODEDynamics: Scaled geometries are not supported'
odegeom.setPosition(pos)
# row major or column major?
odegeom.setRotation(rot.getMat3().toList(rowmajor=True))
return odegeom
