def _createEllipsoid(proxy, radius, colour, moiScale, withMesh): """ Private function. Use createEllipsoid() or createArticulatedEllipsoid() instead. """ # Mesh and cdps will be set manually proxy.meshes = None proxy.cdps = None # Compute box moi moi = [0, 0, 0] for i in range(3): j = (i + 1) % 3 k = (i + 2) % 3 moi[i] = proxy.mass * (radius[j] * radius[j] + radius[k] * radius[k]) / 5.0 proxy.moi = PyUtils.toVector3d(moi) * moiScale ellipsoid = proxy.createAndFillObject() cdp = Physics.SphereCDP() cdp.setCenter(Point3d(0, 0, 0)) cdp.setRadius(min(radius)) ellipsoid.addCollisionDetectionPrimitive(cdp) if withMesh: mesh = Mesh.createEllipsoid((0, 0, 0), radius, colour) ellipsoid.addMesh(mesh) return ellipsoid
def _createCylinder(proxy, axis, basePos, tipPos, radius, colour, moiScale, withMesh): """ Private function. Use createCylinder() or createArticulatedCylinder() instead. """ if axis != 0 and axis != 1 and axis != 2: raise ValueError('Axis must be 0 for x, 1 for y or 2 for z.') # Mesh and cdps will be set manually proxy.meshes = None proxy.cdps = None # Compute box moi moi = [0, 0, 0] height = math.fabs(tipPos - basePos) for i in range(3): if i == axis: moi[i] = proxy.mass * radius * radius / 2.0 else: moi[i] = proxy.mass * (3 * radius * radius + height * height) / 12.0 ### HACK! moi[i] = max(moi[i], 0.01) proxy.moi = PyUtils.toVector3d(moi) * moiScale cylinder = proxy.createAndFillObject() basePoint = [0, 0, 0] tipPoint = [0, 0, 0] basePoint[axis] = basePos tipPoint[axis] = tipPos basePoint3d = PyUtils.toPoint3d(basePoint) tipPoint3d = PyUtils.toPoint3d(tipPoint) baseToTipVector3d = Vector3d(basePoint3d, tipPoint3d) if baseToTipVector3d.isZeroVector(): raise ValueError( 'Invalid points for cylinder: base and tip are equal!') baseToTipUnitVector3d = baseToTipVector3d.unit() if height <= radius * 2.0: cdp = Physics.SphereCDP() cdp.setCenter(basePoint3d + baseToTipVector3d * 0.5) cdp.setRadius(height / 2.0) else: cdp = Physics.CapsuleCDP() cdp.setPoint1(basePoint3d + baseToTipUnitVector3d * radius) cdp.setPoint2(tipPoint3d + baseToTipUnitVector3d * -radius) cdp.setRadius(radius) cylinder.addCollisionDetectionPrimitive(cdp) if withMesh: mesh = Mesh.createCylinder(basePoint, tipPoint, radius, colour) cylinder.addMesh(mesh) return cylinder