def extend_shape(diff_x, diff_y, diff_z):
reset_shape()
# see http://mathworld.wolfram.com/RotationMatrix.html
hypotenuse = sqrt(diff_x * diff_x + diff_y * diff_y)
# Some paths contain two identical points (e.g. a "touch" of the
# PushCutter). We don't need any extension for these.
if hypotenuse == 0:
return
cosinus = diff_x / hypotenuse
sinus = diff_y / hypotenuse
# create the cyclinder at the other end
geom_end_transform = ode.GeomTransform(geom.space)
geom_end_transform.setBody(geom.getBody())
geom_end = ode.GeomCapsule(None, radius, self.height)
geom_end.setPosition((diff_x, diff_y, diff_z + center_height))
geom_end_transform.setGeom(geom_end)
# create the block that connects the two cylinders at the end
rot_matrix_box = (cosinus, sinus, 0.0, -sinus, cosinus, 0.0,
0.0, 0.0, 1.0)
geom_connect_transform = ode.GeomTransform(geom.space)
geom_connect_transform.setBody(geom.getBody())
geom_connect = ode_physics.get_parallelepiped_geom(
(Point(-hypotenuse / 2, radius, -diff_z / 2),
Point(hypotenuse / 2, radius, diff_z / 2),
Point(hypotenuse / 2, -radius, diff_z / 2),
Point(-hypotenuse / 2, -radius, -diff_z / 2)),
(Point(-hypotenuse / 2, radius, self.height - diff_z / 2),
Point(hypotenuse / 2, radius, self.height + diff_z / 2),
Point(hypotenuse / 2, -radius, self.height + diff_z / 2),
Point(-hypotenuse / 2, -radius,
self.height - diff_z / 2)))
geom_connect.setRotation(rot_matrix_box)
geom_connect.setPosition((hypotenuse / 2, 0, radius))
geom_connect_transform.setGeom(geom_connect)
# Create a cylinder, that connects the two half spheres at the
# lower end of both drills.
geom_cyl_transform = ode.GeomTransform(geom.space)
geom_cyl_transform.setBody(geom.getBody())
hypotenuse_3d = Matrix.get_length((diff_x, diff_y, diff_z))
geom_cyl = ode.GeomCylinder(None, radius, hypotenuse_3d)
# rotate cylinder vector
cyl_original_vector = (0, 0, hypotenuse_3d)
cyl_destination_vector = (diff_x, diff_y, diff_z)
matrix = Matrix.get_rotation_matrix_from_to(
cyl_original_vector, cyl_destination_vector)
flat_matrix = matrix[0] + matrix[1] + matrix[2]
geom_cyl.setRotation(flat_matrix)
# The rotation is around the center - thus we ignore negative
# diff values.
geom_cyl.setPosition((abs(diff_x / 2), abs(diff_y / 2),
radius - additional_distance))
geom_cyl_transform.setGeom(geom_cyl)
# sort the geoms in order of collision probability
geom.children.extend([
geom_connect_transform, geom_cyl_transform,
geom_end_transform
])
def extend_shape(diff_x, diff_y, diff_z):
reset_shape()
# see http://mathworld.wolfram.com/RotationMatrix.html
hypotenuse = sqrt(diff_x * diff_x + diff_y * diff_y)
# Some paths contain two identical points (e.g. a "touch" of
# the PushCutter) We don't need any extension for these.
if hypotenuse == 0:
return
cosinus = diff_x / hypotenuse
sinus = diff_y / hypotenuse
# create the cyclinder at the other end
geom_end_transform = ode.GeomTransform(geom.space)
geom_end_transform.setBody(geom.getBody())
geom_end = ode.GeomCylinder(None, radius, height)
geom_end.setPosition((diff_x, diff_y, diff_z + center_height))
geom_end_transform.setGeom(geom_end)
# create the block that connects to two cylinders at the end
rot_matrix_box = (cosinus, sinus, 0.0, -sinus, cosinus, 0.0,
0.0, 0.0, 1.0)
geom_connect_transform = ode.GeomTransform(geom.space)
geom_connect_transform.setBody(geom.getBody())
geom_connect = ode_physics.get_parallelepiped_geom(
((-hypotenuse / 2, radius, -diff_z / 2),
(hypotenuse / 2, radius, diff_z / 2),
(hypotenuse / 2, -radius, diff_z / 2),
(-hypotenuse / 2, -radius, -diff_z / 2)),
((-hypotenuse / 2, radius,
self.height - diff_z / 2),
(hypotenuse / 2,
radius, self.height + diff_z / 2),
(hypotenuse / 2, -radius,
self.height + diff_z / 2),
(-hypotenuse / 2, -radius,
self.height - diff_z / 2)))
geom_connect.setRotation(rot_matrix_box)
geom_connect.setPosition((hypotenuse / 2, 0, radius))
geom_connect_transform.setGeom(geom_connect)
# sort the geoms in order of collision probability
geom.children.extend([geom_connect_transform,
geom_end_transform])
def createGeoms(self, obj, space):
"""Create all the geoms for one world object including the children.
The generated geoms will be encapsulated in GeomTransforms
so that all of them are defined with respect to the pivot coordinate
system of \a obj.
\param obj (\c WorldObject) Top level world object
\param space (\c ode.Space) ODE Space object
\return List of ODE geom objects
"""
# Plane? This is a special case because ODE planes are not placeable
if isinstance(obj.geom, PlaneGeom):
if not obj.static:
raise RuntimeError, "Planes can only be used as static bodies"
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)
return [odegeom]
res = []
# Create all ODE geoms in the subtree starting at obj
# Each geom will have a position/rotation that moves it to
# the local coordinate system of obj.
geoms = self._createLGeoms(obj, mat4(1))
# Apply the offset transform and encapsulate the geoms inside
# geom transforms...
P = obj.getOffsetTransform()
Pinv = P.inverse()
pos, rot, scale = Pinv.decompose()
if scale != vec3(1, 1, 1):
print 'WARNING: ODEDynamics: Scaled geometries are not supported'
res = []
for g in geoms:
M = mat4(1).translation(vec3(g.getPosition()))
M.setMat3(mat3(g.getRotation()))
M *= Pinv
p4 = M.getColumn(3)
g.setPosition((p4.x, p4.y, p4.z))
# row major or column major?
g.setRotation(M.getMat3().toList(rowmajor=True))
gt = ode.GeomTransform(space)
gt.setGeom(g)
res.append(gt)
return res
def construct(self):
self.body = ode.Body(world)
M = ode.Mass()
M.setCappedCylinderTotal(self.mass, 1, self.radius, self.length)
self.body.setMass(M)
self.body.setPosition((self.x, self.y, 0))
self.setRotation(self.start_angle)
self.geom = ode.GeomTransform(space)
self.geom2 = ode.GeomCapsule(None, self.radius,
self.length) # by default points into z
self.geom2.setRotation(
(0, 0, 1, 1, 0, 0, 0, 1, 0)) # make z point into x
self.geom.setBody(self.body)
self.geom.setGeom(self.geom2)
ODEThing.construct(self)
def __init__(self,
world,
space,
pos,
color,
capsules,
radius,
mass=2,
fixed=False,
orientation=v(1, 0, 0, 0)):
"capsules is a list of (start, end) points"
self.capsules = capsules
self.body = ode.Body(world)
self.body.setPosition(pos)
self.body.setQuaternion(orientation)
m = ode.Mass()
# computing MOI assuming sphere with .5 m radius
m.setSphere(mass / (4 / 3 * math.pi * .5**3),
.5) # setSphereTotal is broken
self.body.setMass(m)
self.geoms = []
self.geoms2 = []
for start, end in capsules:
self.geoms.append(ode.GeomTransform(space))
x = ode.GeomCapsule(None, radius, (end - start).mag())
self.geoms2.append(x)
self.geoms[-1].setGeom(x)
self.geoms[-1].setBody(self.body)
x.setPosition((start + end) / 2 + v(random.gauss(
0, .01), random.gauss(0, .01), random.gauss(0, .01)))
a = (end - start).unit()
b = v(0, 0, 1)
x.setQuaternion(
sim_math_helpers.axisangle_to_quat((a % b).unit(),
-math.acos(a * b)))
self.color = color
self.radius = radius
if fixed:
self.joint = ode.FixedJoint(world)
self.joint.attach(self.body, None)
self.joint.setFixed()
def _setObject(self, kclass, **kwargs):
"""
Create the Geom object and apply transforms. Only call for placeable
Geoms.
"""
if (self._body is None):
# The Geom is independant so it can have its own transform
kwargs['space'] = self._space
obj = kclass(**kwargs)
t = self.getTransform()
obj.setPosition(t.getPosition())
obj.setRotation(t.getRotation())
self.setODEObject(obj)
elif (self._transformed):
# The Geom is attached to a body so to transform it, it must
# by placed in a GeomTransform and its transform is relative
# to the body.
kwargs['space'] = None
obj = kclass(**kwargs)
t = self.getTransform(self._body)
obj.setPosition(t.getPosition())
obj.setRotation(t.getRotation())
trans = ode.GeomTransform(self._space)
trans.setGeom(obj)
trans.setBody(self._body.getODEObject())
self.setODEObject(trans)
else:
kwargs['space'] = self._space
obj = kclass(**kwargs)
obj.setBody(self._body.getODEObject())
self.setODEObject(obj)
def get_shape(self, engine="ODE"):
if engine == "ODE":
import ode
import pycam.Physics.ode_physics as ode_physics
""" We don't handle the the "additional_distance" perfectly, since
the "right" shape would be a cylinder with a small flat cap that
grows to the full expanded radius through a partial sphere. The
following ascii art shows the idea:
| |
\_/
This slight incorrectness should be neglectable and causes no harm.
"""
additional_distance = self.get_required_distance()
radius = self.distance_radius
height = self.height + additional_distance
center_height = height / 2 - additional_distance
geom = ode.GeomTransform(None)
geom_drill = ode.GeomCylinder(None, radius, height)
geom_drill.setPosition((0, 0, center_height))
geom.setGeom(geom_drill)
geom.children = []
def reset_shape():
geom.children = []
def set_position(x, y, z):
geom.setPosition((x, y, z))
def extend_shape(diff_x, diff_y, diff_z):
reset_shape()
# see http://mathworld.wolfram.com/RotationMatrix.html
hypotenuse = sqrt(diff_x * diff_x + diff_y * diff_y)
# Some paths contain two identical points (e.g. a "touch" of
# the PushCutter) We don't need any extension for these.
if hypotenuse == 0:
return
cosinus = diff_x / hypotenuse
sinus = diff_y / hypotenuse
# create the cyclinder at the other end
geom_end_transform = ode.GeomTransform(geom.space)
geom_end_transform.setBody(geom.getBody())
geom_end = ode.GeomCylinder(None, radius, height)
geom_end.setPosition((diff_x, diff_y, diff_z + center_height))
geom_end_transform.setGeom(geom_end)
# create the block that connects to two cylinders at the end
rot_matrix_box = (cosinus, sinus, 0.0, -sinus, cosinus, 0.0,
0.0, 0.0, 1.0)
geom_connect_transform = ode.GeomTransform(geom.space)
geom_connect_transform.setBody(geom.getBody())
geom_connect = ode_physics.get_parallelepiped_geom(
(Point(-hypotenuse / 2, radius, -diff_z / 2),
Point(hypotenuse / 2, radius, diff_z / 2),
Point(hypotenuse / 2, -radius, diff_z / 2),
Point(-hypotenuse / 2, -radius, -diff_z / 2)),
(Point(-hypotenuse / 2, radius, self.height - diff_z / 2),
Point(hypotenuse / 2, radius, self.height + diff_z / 2),
Point(hypotenuse / 2, -radius, self.height + diff_z / 2),
Point(-hypotenuse / 2, -radius,
self.height - diff_z / 2)))
geom_connect.setRotation(rot_matrix_box)
geom_connect.setPosition((hypotenuse / 2, 0, radius))
geom_connect_transform.setGeom(geom_connect)
# sort the geoms in order of collision probability
geom.children.extend(
[geom_connect_transform, geom_end_transform])
geom.extend_shape = extend_shape
geom.reset_shape = reset_shape
self.shape[engine] = (geom, set_position)
return self.shape[engine]