def __init__(self, scalenode, shapenode):
d = []
self.data = d
self._scalenode = scalenode
self._shapenode = shapenode
wt = scalenode.get_world_transform()
v0 = wt * vec4(1, 0, 0, 0)
v1 = wt * vec4(0, 1, 0, 0)
v2 = wt * vec4(0, 0, 1, 0)
nv0 = v0.normalize()
nv1 = v1.normalize()
nv2 = v2.normalize()
d0, d1, d2 = map(lambda x, y: math.fabs(x * y), [nv0, nv0, nv1],
[nv1, nv2, nv2])
if d0 > 0.1 or d1 > 0.1 or d2 > 0.1:
print("Error: scale for physical shape '%s' is not orthogonal!" %
scalenode.getFullName())
print(nv0, nv1, nv2)
print(d0, d1, d2)
print(wt)
sys.exit(21)
check_orthonormal = True
if shapenode.nodetype == "polyCube":
self.type = "box"
check_orthonormal = False
scale = scalenode.get_world_scale()
x = shapenode.getAttrValue("w", "w", None, default=1.0)
y = shapenode.getAttrValue("h", "h", None, default=1.0)
z = shapenode.getAttrValue("d", "d", None, default=1.0)
d += list(map(lambda x, y: x * y, [x, y, z], scale))
elif shapenode.nodetype == "polySphere":
self.type = "sphere"
d.append(
shapenode.getAttrValue("r", "r", None, default=1.0) *
v0.length())
else:
print(
"Error: primitive physics shape type '%s' on node '%s' is unknown."
% (shapenode.nodetype, shapenode.getFullName()))
sys.exit(22)
if check_orthonormal:
if math.fabs(v0.length() - v1.length()) > 0.1 or math.fabs(
v0.length() -
v2.length()) > 0.1 or math.fabs(v1.length() -
v2.length()) > 0.1:
print(
"Error: scale for physical shape '%s' is not orthonormal!"
% scalenode.getFullName())
sys.exit(21)
def dowrite(self):
self._listchildmeshes()
filename = self.basename+".class"
with self._fileopenwrite(filename) as f:
self.f = f
meshptrs = []
physidx = 0
for m in self.meshes:
def _getparentphys(m):
ph = None
mesh = m
while mesh and not ph:
for phys in self.bodies:
meshes = list(filter(None, [ch == mesh for ch in phys.childmeshes]))
if len(meshes) == 1:
if not ph:
ph = phys
else:
print("Error: both phys %s and %s has mesh refs to %s." % (ph.getFullName(), phys.getFullName(), mesh.getFullName()))
print(ph.childmeshes, meshes)
sys.exit(3)
elif len(meshes) > 1:
print("Error: phys %s has multiple mesh children refs to %s." % (phys.getFullName(), mesh.getFullName()))
sys.exit(3)
mesh = mesh.getParent()
if not ph:
print("Warning: mesh %s is not attached to any physics object!" % m.getFullName())
return ph
phys = _getparentphys(m)
if not phys:
continue
phys.writecount = 1
m.writecount += 1
tm = m.get_world_transform()
tp = phys.get_world_transform()
tmt, tmr, _ = tm.decompose()
tpt, tpr, _ = tp.decompose()
q = quat(tpr.inverse()).normalize()
p = tmt-tpt
p = q.toMat4() * vec4(p[:])
q = quat(tpr.inverse() * tmr).normalize()
p = p[0:3]
t = self._normalizexform(q[:]+p[:])
physidx = self.bodies.index(phys)
meshptrs += [(CHUNK_CLASS_PHYS_MESH, PhysMeshPtr(physidx, m.meshbasename, t, m.mat))]
data = (
CHUNK_CLASS,
(
(CHUNK_CLASS_PHYSICS, self.basename),
(CHUNK_CLASS_MESH_LIST, meshptrs),
)
)
#pprint.pprint(data)
self._writechunk(data)
self._addfeat("class:classes", 1)
self._verifywritten("physics->mesh links", self.meshes)
self._addfeat("file:files", 1)
def __init__(self, scalenode, shapenode):
d = []
self.data = d
self._scalenode = scalenode
self._shapenode = shapenode
wt = scalenode.get_world_transform()
v0 = wt*vec4(1,0,0,0)
v1 = wt*vec4(0,1,0,0)
v2 = wt*vec4(0,0,1,0)
nv0 = v0.normalize()
nv1 = v1.normalize()
nv2 = v2.normalize()
d0, d1, d2 = map(lambda x,y: math.fabs(x*y), [nv0, nv0, nv1], [nv1, nv2, nv2])
if d0 > 0.1 or d1 > 0.1 or d2 > 0.1:
print("Error: scale for physical shape '%s' is not orthogonal!" % scalenode.getFullName())
print(nv0, nv1, nv2)
print(d0, d1, d2)
print(wt)
sys.exit(21)
check_orthonormal = True
if shapenode.nodetype == "polyCube":
self.type = "box"
check_orthonormal = False
scale = scalenode.get_world_scale()
x = shapenode.getAttrValue("w", "w", None, default=1.0)
y = shapenode.getAttrValue("h", "h", None, default=1.0)
z = shapenode.getAttrValue("d", "d", None, default=1.0)
d += list(map(lambda x,y: x*y, [x,y,z], scale))
elif shapenode.nodetype == "polySphere":
self.type = "sphere"
d.append(shapenode.getAttrValue("r", "r", None, default=1.0)*v0.length())
else:
print("Error: primitive physics shape type '%s' on node '%s' is unknown." % (shapenode.nodetype, shapenode.getFullName()))
sys.exit(22)
if check_orthonormal:
if math.fabs(v0.length()-v1.length()) > 0.1 or math.fabs(v0.length()-v2.length()) > 0.1 or math.fabs(v1.length()-v2.length()) > 0.1:
print("Error: scale for physical shape '%s' is not orthonormal!" % scalenode.getFullName())
sys.exit(21)
def _writeshape(self, shape):
# Write all general parameters first.
types = {"capsule":1, "cylinder":2, "sphere":3, "box":4, "mesh":5}
self._writeint(types[shape.type])
node = shape.getnode()
totalmass = self._gettotalmass()
self._writefloat(float(node.get_fixed_attribute("mass")))
friction = float(node.get_fixed_attribute("friction"))
if self.config["type"] == "dynamic":
friction *= totalmass / 1000.0
self._writefloat(friction)
bounce = node.get_fixed_attribute("bounce")
self._writefloat(float(bounce))
if options.options.verbose:
print("Writing shape %s friction=%f, bounce=%f." % (node.getName(), friction, bounce))
rootindex = -1 if not node.phys_root else self.bodies.index(node.phys_root)
self._writeint(rootindex)
jointtype = node.get_fixed_attribute("joint", True)
jointvalue = joints[jointtype]
#print(node.getName(), "is jointed by type", jointvalue)
if jointtype:
self._addfeat("joint:joints", 1)
self._writeint(jointvalue)
is_affected_by_gravity = 1 if node.get_fixed_attribute("affected_by_gravity") else 0
if options.options.verbose and is_affected_by_gravity:
print("Writing shape %s as affected by gravity." % node.getName())
self._writeint(is_affected_by_gravity)
type_body, type_trigger, type_position = 1, 2, 3
if node.getName().startswith("phys_trig_"):
phys_type = type_trigger
if options.options.verbose:
print("Writing shape %s as trigger." % node.getName())
elif node.getName().startswith("phys_pos_"):
phys_type = type_position
if options.options.verbose:
print("Writing shape %s as position." % node.getName())
else:
phys_type = type_body
self._writeint(phys_type)
# Write joint parameters.
parameters = [0.0]*16
#print("Total mass:", totalmass)
parameters[0] = node.get_fixed_attribute("joint_spring_constant", True, 0.0) * totalmass
parameters[1] = node.get_fixed_attribute("joint_spring_damping", True, 0.0) * totalmass
ir = node.getabsirot()
yaw = node.get_fixed_attribute("joint_yaw", True, 0.0)*math.pi/180
pitch = node.get_fixed_attribute("joint_pitch", True, 0.0)*math.pi/180
if options.options.verbose and jointvalue >= 2:
print("Joint %s euler angles are %s, damping is %f." % (node.getName(), (yaw, pitch), parameters[1]))
parameters[2:4] = yaw, pitch
joint_min, joint_max = node.get_fixed_attribute("joint_stops", True, [0.0,0.0])
if jointtype == "slider":
parameters[4:6] = joint_min, joint_max
else:
parameters[4:6] = math.radians(joint_min), math.radians(joint_max)
mp = node.get_world_pivot_transform()
mt = node.get_world_transform()
wp = mp * vec4(0,0,0,1)
wt = mt * vec4(0,0,0,1)
#j = wt-wp
j = wp-wt
j = ir * j
if node.is_phys_root and node.xformparent == node.getphysmaster():
j = node._pointup_adjust_mat() * j
j -= self.physrootpos
#print(node.getName(), "\n", mp, "\n", mt, "\n", wp, "\n", wt, "\n", ir, "\n", j)
parameters[6:9] = j[:3]
parameters[9] = node.get_fixed_attribute("impact_factor", True, 1.0)
parameters[10] = 1.0 if node.get_fixed_attribute("collide_with_self", True, False) else 0.0
detachable = any(node == mesh.get_mesh_parentphys(self.bodies) for mesh in self.meshes)
if options.options.verbose:
print('%s %s detachable.' % (node.getFullName(), 'is' if detachable else 'is not'))
parameters[11] = 1.0 if detachable else 0.0
if options.options.verbose:
print("%s parameters:" % node.getName(), parameters)
for x in parameters:
self._writefloat(float(x))
# Write connecor type (may hook other stuff, may get hooked by hookers :).
connectors = node.get_fixed_attribute("connector_types", True)
if connectors:
if not type(connectors) == list and not type(connetors) == tuple:
print("Error: connector_types for '%s' must be a list." % node.getFullName())
sys.exit(19)
self._writeint(len(connectors))
for c in connectors:
self._writeint(connector_types[c])
else:
self._writeint(0)
# Write material.
material = node.get_fixed_attribute("material")
if options.options.verbose:
print("Writing physical material", material);
self._writestr(material)
# Write shape data (dimensions of shape).
scale = node.get_fixed_attribute("scale", default=1)
shape.data = [type(x)(scale*x) for x in shape.data]
if options.options.verbose:
print("Writing shape %s with rootindex %i: %s (scale=%f)." % (node.getName(), rootindex, str(shape.data), scale))
if node == self.bodies[0]:
shape.adjustmesh(self.physrootpos)
for x in shape.data:
self._writenumber(x)
self._addfeat("physical geometry:physical geometries", 1)
def dowrite(self):
self._listchildmeshes()
filename = self.basename + ".class"
with self._fileopenwrite(filename) as f:
self.f = f
meshptrs = []
physidx = 0
for m in self.meshes:
def _getparentphys(m):
ph = None
mesh = m
while mesh and not ph:
for phys in self.bodies:
meshes = list(
filter(None,
[ch == mesh
for ch in phys.childmeshes]))
if len(meshes) == 1:
if not ph:
ph = phys
else:
print(
"Error: both phys %s and %s has mesh refs to %s."
%
(ph.getFullName(), phys.getFullName(),
mesh.getFullName()))
print(ph.childmeshes, meshes)
sys.exit(3)
elif len(meshes) > 1:
print(
"Error: phys %s has multiple mesh children refs to %s."
% (phys.getFullName(), mesh.getFullName()))
sys.exit(3)
mesh = mesh.getParent()
if not ph:
print(
"Warning: mesh %s is not attached to any physics object!"
% m.getFullName())
return ph
phys = _getparentphys(m)
if not phys:
continue
phys.writecount = 1
m.writecount += 1
tm = m.get_world_transform()
tp = phys.get_world_transform()
tmt, tmr, _ = tm.decompose()
tpt, tpr, _ = tp.decompose()
q = quat(tpr.inverse()).normalize()
p = tmt - tpt
p = q.toMat4() * vec4(p[:])
q = quat(tpr.inverse() * tmr).normalize()
p = p[0:3]
t = self._normalizexform(q[:] + p[:])
physidx = self.bodies.index(phys)
meshptrs += [(CHUNK_CLASS_PHYS_MESH,
PhysMeshPtr(physidx, m.meshbasename, t, m.mat))]
data = (CHUNK_CLASS, (
(CHUNK_CLASS_PHYSICS, self.basename),
(CHUNK_CLASS_MESH_LIST, meshptrs),
))
#pprint.pprint(data)
self._writechunk(data)
self._addfeat("class:classes", 1)
self._verifywritten("physics->mesh links", self.meshes)
self._addfeat("file:files", 1)
def _writeshape(self, shape):
# Write all general parameters first.
types = {"capsule": 1, "sphere": 2, "box": 3}
self._writeint(types[shape.type])
node = shape.getnode()
totalmass = self._gettotalmass()
self._writefloat(float(node.get_fixed_attribute("mass")))
friction = node.get_fixed_attribute("friction") * totalmass / 1000.0
self._writefloat(friction)
self._writefloat(float(node.get_fixed_attribute("bounce")))
rootindex = -1 if not node.phys_root else self.bodies.index(
node.phys_root)
self._writeint(rootindex)
joints = {
None: 1,
"exclude": 1,
"suspend_hinge": 2,
"hinge2": 3,
"hinge": 4,
"ball": 5,
"universal": 6
}
jointtype = node.get_fixed_attribute("joint", True)
jointvalue = joints[jointtype]
#print(node.getName(), "is jointed by type", jointvalue)
if jointtype:
self._addfeat("joint:joints", 1)
self._writeint(jointvalue)
self._writeint(
1 if node.get_fixed_attribute("affected_by_gravity") else 0)
# Write joint parameters.
parameters = [0.0] * 16
#print("Total mass:", totalmass)
parameters[0] = node.get_fixed_attribute("joint_spring_constant", True,
0.0) * totalmass
parameters[1] = node.get_fixed_attribute("joint_spring_damping", True,
0.0) * totalmass
ir = node.getabsirot()
yaw = node.get_fixed_attribute("joint_yaw", True, 0.0) * math.pi / 180
pitch = node.get_fixed_attribute("joint_pitch", True,
0.0) * math.pi / 180
## m = mat4.identity()
## m.setMat3(mat3.fromEulerXZY(0, pitch, yaw))
## v = ir * m * vec4(0,0,1,0)
## # Project onto XY plane.
## xyv = vec3(v[:3])
## xyv[2] = 0
## if xyv.length() < 1e-12:
## yaw = 0
## if v[2] < 0:
## pitch = math.pi
## else:
## pitch = 0
## else:
## xyv = xyv.normalize()
## # Yaw is angle of projection on the XY plane.
## if xyv.length():
## yaw = math.asin(xyv.y/xyv.length())
## else:
## yaw = 0
## v = v.normalize()
## v = vec3(v[:3])
## pitch = math.asin(v*xyv)
if options.options.verbose and jointvalue >= 2:
print("Joint %s euler angles are: %s." % (node.getName(),
(yaw, pitch)))
parameters[2:4] = yaw, pitch
joint_min, joint_max = node.get_fixed_attribute(
"joint_angles", True, [0.0, 0.0])
parameters[4:6] = math.radians(joint_min), math.radians(joint_max)
mp = node.get_world_pivot_transform()
mt = node.get_world_transform()
wp = mp * vec4(0, 0, 0, 1)
wt = mt * vec4(0, 0, 0, 1)
j = wp - wt
j = ir * j
parameters[6:9] = j[:3]
for x in parameters:
self._writefloat(float(x))
# Write connecor type (may hook other stuff, may get hooked by hookers :).
connectors = node.get_fixed_attribute("connector_types", True)
if connectors:
if not type(connectors) == list and not type(connetors) == tuple:
print("Error: connector_types for '%s' must be a list." %
node.getFullName())
sys.exit(19)
self._writeint(len(connectors))
connector_types = {"connector_3dof": 1, "connectee_3dof": 2}
for c in connectors:
self._writeint(connector_types[c])
else:
self._writeint(0)
# Write shape data (dimensions of shape).
if options.options.verbose:
print("Writing shape %s with rootindex %i." %
(node.getName(), rootindex))
for x in shape.data:
self._writefloat(math.fabs(x))
self._addfeat("physical geometry:physical geometries", 1)
def _writeshape(self, shape):
# Write all general parameters first.
types = {"capsule":1, "sphere":2, "box":3}
self._writeint(types[shape.type])
node = shape.getnode()
totalmass = self._gettotalmass()
self._writefloat(float(node.get_fixed_attribute("mass")))
friction = node.get_fixed_attribute("friction") * totalmass / 1000.0
self._writefloat(friction)
self._writefloat(float(node.get_fixed_attribute("bounce")))
rootindex = -1 if not node.phys_root else self.bodies.index(node.phys_root)
self._writeint(rootindex)
joints = {None:1, "exclude":1, "suspend_hinge":2, "hinge2":3, "hinge":4, "ball":5, "universal":6}
jointtype = node.get_fixed_attribute("joint", True)
jointvalue = joints[jointtype]
#print(node.getName(), "is jointed by type", jointvalue)
if jointtype:
self._addfeat("joint:joints", 1)
self._writeint(jointvalue)
self._writeint(1 if node.get_fixed_attribute("affected_by_gravity") else 0)
# Write joint parameters.
parameters = [0.0]*16
#print("Total mass:", totalmass)
parameters[0] = node.get_fixed_attribute("joint_spring_constant", True, 0.0) * totalmass
parameters[1] = node.get_fixed_attribute("joint_spring_damping", True, 0.0) * totalmass
ir = node.getabsirot()
yaw = node.get_fixed_attribute("joint_yaw", True, 0.0)*math.pi/180
pitch = node.get_fixed_attribute("joint_pitch", True, 0.0)*math.pi/180
## m = mat4.identity()
## m.setMat3(mat3.fromEulerXZY(0, pitch, yaw))
## v = ir * m * vec4(0,0,1,0)
## # Project onto XY plane.
## xyv = vec3(v[:3])
## xyv[2] = 0
## if xyv.length() < 1e-12:
## yaw = 0
## if v[2] < 0:
## pitch = math.pi
## else:
## pitch = 0
## else:
## xyv = xyv.normalize()
## # Yaw is angle of projection on the XY plane.
## if xyv.length():
## yaw = math.asin(xyv.y/xyv.length())
## else:
## yaw = 0
## v = v.normalize()
## v = vec3(v[:3])
## pitch = math.asin(v*xyv)
if options.options.verbose and jointvalue >= 2:
print("Joint %s euler angles are: %s." % (node.getName(), (yaw, pitch)))
parameters[2:4] = yaw, pitch
joint_min, joint_max = node.get_fixed_attribute("joint_angles", True, [0.0,0.0])
parameters[4:6] = math.radians(joint_min), math.radians(joint_max)
mp = node.get_world_pivot_transform()
mt = node.get_world_transform()
wp = mp * vec4(0,0,0,1)
wt = mt * vec4(0,0,0,1)
j = wp-wt
j = ir * j
parameters[6:9] = j[:3]
for x in parameters:
self._writefloat(float(x))
# Write connecor type (may hook other stuff, may get hooked by hookers :).
connectors = node.get_fixed_attribute("connector_types", True)
if connectors:
if not type(connectors) == list and not type(connetors) == tuple:
print("Error: connector_types for '%s' must be a list." % node.getFullName())
sys.exit(19)
self._writeint(len(connectors))
connector_types = {"connector_3dof":1, "connectee_3dof":2}
for c in connectors:
self._writeint(connector_types[c])
else:
self._writeint(0)
# Write shape data (dimensions of shape).
if options.options.verbose:
print("Writing shape %s with rootindex %i." % (node.getName(), rootindex))
for x in shape.data:
self._writefloat(math.fabs(x))
self._addfeat("physical geometry:physical geometries", 1)
def __init__(self, scalenode, shapenode):
d = []
self.data = d
self._scalenode = scalenode
self._shapenode = shapenode
scalenode.pointup = False
wt = scalenode.get_world_transform()
v0 = wt*vec4(1,0,0,0)
v1 = wt*vec4(0,1,0,0)
v2 = wt*vec4(0,0,1,0)
nv0 = v0.normalize()
nv1 = v1.normalize()
nv2 = v2.normalize()
d0, d1, d2 = map(lambda x,y: math.fabs(x*y), [nv0, nv0, nv1], [nv1, nv2, nv2])
global disable_ortho_check
if not disable_ortho_check and (d0 > 0.1 or d1 > 0.1 or d2 > 0.1):
print("Error: scale for physical shape '%s' is not orthogonal!" % scalenode.getFullName())
print(nv0, nv1, nv2)
print(d0, d1, d2)
print(wt)
sys.exit(21)
check_orthonormal = True
if shapenode.nodetype == "polyCube":
self.type = "box"
check_orthonormal = False
scale = scalenode.get_world_scale()
x = shapenode.getAttrValue("w", "w", None, default=1.0)
y = shapenode.getAttrValue("h", "h", None, default=1.0)
z = shapenode.getAttrValue("d", "d", None, default=1.0)
d += list(map(lambda x,y: x*y, [x,y,z], scale))
elif shapenode.nodetype == "polySphere":
self.type = "sphere"
d.append(shapenode.getAttrValue("r", "r", None, default=1.0)*v0.length())
elif shapenode.nodetype == "polyCylinder":
self.type = "cylinder"
scalenode.pointup = True
r = shapenode.getAttrValue("r", "r", None, default=1.0)*v0.length()
h = shapenode.getAttrValue("h", "h", None, default=2.0)*v0.length()
d += [r, h]
elif shapenode.nodetype == "mesh":
self.type = "mesh"
vtx = shapenode.get_fixed_attribute("rgvtx")
tri = shapenode.get_fixed_attribute("rgtri")
d += [len(vtx)//3, len(tri)//3]
d += list(map(lambda x: float(x), vtx))
d += list(map(lambda x: int(x), tri))
else:
print("Error: primitive physics shape type '%s' on node '%s' is unknown." % (shapenode.nodetype, shapenode.getName()))
sys.exit(22)
override_shapetype = scalenode.get_fixed_attribute("override_shapetype", optional=True)
if override_shapetype:
self.type = override_shapetype
if self.type == "capsule":
r = shapenode.getAttrValue("r", "r", None, default=1.0)*v0.length()
# Reduce height by the radius in each end, so it won't bulge out.
h = shapenode.getAttrValue("h", "h", None, default=2.0)*v0.length() - 2*r
if r <= 0 or h <= 0:
print("Error: the %s shape is used as a capsule for %s, and must have a greater height than radius*2. (r=%f, h=%f)" %
(shapenode.nodetype, scalenode.getFullName(), r, h))
d[-1] = h
if not disable_ortho_check and check_orthonormal:
if math.fabs(v0.length()-v1.length()) > 0.1 or math.fabs(v0.length()-v2.length()) > 0.1 or math.fabs(v1.length()-v2.length()) > 0.1:
print("Error: scale for physical shape '%s' is not orthonormal!" % scalenode.getFullName())
print(v0, v1, v2)
print(scalenode, scalenode.get_world_transform())
print(disable_ortho_check)
sys.exit(21)
