private void LoadLink(UrdfLink link, string baseDirectory) { float mass = 0; UrdfInertial inertial = link.Inertial; if (inertial != null) { mass = (float)inertial.Mass; } UrdfCollision collision = link.Collision; if (collision != null) { Matrix origin = ParsePose(collision.Origin); UrdfGeometry geometry = collision.Geometry; switch (geometry.Type) { case UrdfGeometryType.Box: var box = geometry as UrdfBox; Vector3 size = ParseVector3(box.Size); var boxShape = new BoxShape(size * 0.5f); PhysicsHelper.CreateBody(mass, origin, boxShape, World); break; case UrdfGeometryType.Cylinder: var cylinder = geometry as UrdfCylinder; float radius = (float)cylinder.Radius * 0.5f; float length = (float)cylinder.Length * 0.5f; var cylinderShape = new CylinderShape(radius, length, radius); PhysicsHelper.CreateBody(mass, origin, cylinderShape, World); break; case UrdfGeometryType.Mesh: var mesh = geometry as UrdfMesh; LoadFile(mesh.FileName, baseDirectory, origin); break; case UrdfGeometryType.Sphere: var sphere = geometry as UrdfSphere; var sphereShape = new SphereShape((float)sphere.Radius); PhysicsHelper.CreateBody(mass, origin, sphereShape, World); break; } } }
private CollisionShape CreateShapeFromGeometry(UrdfGeometry geometry, float mass, string baseDirectory) { CollisionShape shape; switch (geometry.Type) { case UrdfGeometryType.Box: shape = CreateBoxShape((UrdfBox)geometry); break; case UrdfGeometryType.Capsule: shape = CreateCapsuleShape((UrdfCapsule)geometry); break; case UrdfGeometryType.Cylinder: shape = CreateCylinderShape((UrdfCylinder)geometry); break; case UrdfGeometryType.Mesh: var mesh = geometry as UrdfMesh; Vector3 scale; if (mesh.Scale != null) { scale = ParseVector3(mesh.Scale); } else { scale = Vector3.One; } shape = LoadShapeFromFile(mesh.FileName, mass, scale, baseDirectory); break; case UrdfGeometryType.Plane: shape = CreatePlaneShape((UrdfPlane)geometry); break; case UrdfGeometryType.Sphere: shape = CreateSphereShape((UrdfSphere)geometry); break; default: throw new NotSupportedException(); } return(shape); }
private void LoadLink(UrdfLink link, Matrix parentTransform, string baseDirectory) { float mass = 0; UrdfInertial inertial = link.Inertial; if (inertial != null) { mass = (float)inertial.Mass; } Matrix worldTransform = parentTransform; UrdfCollision collision = link.Collision; if (collision != null) { Matrix origin = ParsePose(collision.Origin); worldTransform = worldTransform * origin; UrdfGeometry geometry = collision.Geometry; CollisionShape shape = CreateShapeFromGeometry(baseDirectory, mass, geometry); RigidBody body; if (mass == 0) { body = PhysicsHelper.CreateStaticBody(worldTransform, shape, World); } else { body = PhysicsHelper.CreateBody(mass, worldTransform, shape, World); } _linkToRigidBody[link.Name] = body; } var children = _linkToParentJoint.Where(l => l.Value?.Parent == link); foreach (KeyValuePair <UrdfLink, UrdfJoint> child in children) { LoadLink(child.Key, worldTransform, baseDirectory); LoadJoint(child.Value); } }
private CollisionShape CreateShapeFromGeometry(string baseDirectory, float mass, UrdfGeometry geometry) { CollisionShape shape; switch (geometry.Type) { case UrdfGeometryType.Box: shape = CreateBoxShape((UrdfBox)geometry); break; case UrdfGeometryType.Capsule: shape = CreateCapsuleShape((UrdfCapsule)geometry); break; case UrdfGeometryType.Cylinder: shape = CreateCylinderShape((UrdfCylinder)geometry); break; case UrdfGeometryType.Mesh: var mesh = geometry as UrdfMesh; shape = LoadShapeFromFile(mesh.FileName, baseDirectory); break; case UrdfGeometryType.Sphere: shape = CreateSphereShape((UrdfSphere)geometry); break; default: throw new NotSupportedException(); } return(shape); }