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 create_capsule(self, density, r, h):
body = ode.Body(self.world)
M = ode.Mass()
M.setCapsule(density, 1, r, h)
body.setMass(M)
geom = ode.GeomCapsule(self.space[0], radius=r, length=h)
geom.setBody(body)
return body, geom
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 make_capsule(self, world, space):
direction, radius, length = (self.parameters['direction'],
self.parameters['radius'],
self.parameters['length'])
# Create body
body = ode.Body(world)
M = ode.Mass()
M.setCapsule(self.parameters['density'], direction, radius, length)
body.setMass(M)
# Create a box geom for collision detection
geom = ode.GeomCapsule(space, radius=radius, length=length)
geom.setBody(body)
return body, geom
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()
You should have received a copy of the GNU General Public License
along with PyCAM. If not, see <http://www.gnu.org/licenses/>.
"""
from pycam.Geometry.Triangle import Triangle
from pycam.Geometry.utils import number
import uuid
try:
import ode
except ImportError:
ode = None
ShapeCylinder = lambda radius, height: ode.GeomCylinder(None, radius, height)
ShapeCapsule = lambda radius, height: \
ode.GeomCapsule(None, radius, height - (2 * radius))
_ode_override_state = None
def generate_physics(models, cutter, physics=None):
if physics is None:
physics = PhysicalWorld()
physics.reset()
if not isinstance(models, (list, set, tuple)):
models = [models]
for model in models:
physics.add_mesh(model.triangles())
shape_info = cutter.get_shape("ODE")
physics.set_drill(shape_info[0], (0.0, 0.0, 0.0))
return physics