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);
        }