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