<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">

    <xacro:macro name="sphere_inertia" params="mass radius">
        <inertial>
            <mass value="${mass}"/>
            <origin xyz="0 0 0"/>

            <inertia ixx="${2*mass*radius*radius/5}" ixy="0.0" ixz="0.0"
                     iyy="${2*mass*radius*radius/5}" iyz="0.0"
                     izz="${2*mass*radius*radius/5}"/>
        </inertial>
    </xacro:macro>

    <xacro:macro name="sphere_inertia_gazebo" params="mass radius">
        <inertial>
            <mass>${mass}</mass>
            <inertia>
                <ixx>${2*mass*radius*radius/5}</ixx>
                <ixy>0</ixy>
                <iyy>${2*mass*radius*radius/5}</iyy>
                <ixz>0</ixz>
                <iyz>0</iyz>
                <izz>${2*mass*radius*radius/5}</izz>
            </inertia>
        </inertial>
    </xacro:macro>

    <xacro:macro name="sphere_geometry" params="radius">
        <geometry>
            <sphere radius="${radius}"/>
        </geometry>
    </xacro:macro>

    <xacro:macro name="sphere_geometry_gazebo" params="radius">
        <geometry>
            <sphere>
                <radius>${radius}</radius>
            </sphere>
        </geometry>
    </xacro:macro>



    <xacro:macro name="sphere_collision" params="radius bit:=0xffff">
        <collision>
	    <xacro:sphere_geometry radius="${radius}"/>
            <xacro:bitwise bit="${bit}"/>
        </collision>
    </xacro:macro>
    
    <xacro:macro name="cylinder_inertia" params="mass radius height x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0">
        <inertial>
            <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}"/>
            <mass value="${mass}"/>
            <origin xyz="0 0 0"/>
            <inertia ixx="${(3*radius*radius + height*height)*mass/12}" ixy="0.0" ixz="0.0"
                     iyy="${(3*radius*radius + height*height)*mass/12}" iyz="0.0"
                     izz="${radius*radius*mass/2}"/>
        </inertial>
    </xacro:macro>

    <xacro:macro name="cylinder_geometry" params="radius height">
        <geometry>
            <cylinder length="${height}" radius="${radius}"/>
        </geometry>
    </xacro:macro>

    <xacro:macro name="cylinder_collision" params="radius height bit:=0xffff">
        <collision>
            <xacro:cylinder_geometry radius="${radius}" height="${height}"/>
            <xacro:bitwise bit="${bit}"/>
        </collision>
    </xacro:macro>

    <xacro:macro name="box_inertia" params="mass width length height x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0">
        <inertial>
            <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}"/>
            <mass value="${mass}"/>
            <inertia ixx="${(height*height + length*length)*mass/12}" ixy="0.0" ixz="0.0"
                     iyy="${(height*height + width*width  )*mass/12}" iyz="0.0"
                     izz="${(width*width + length*length)*mass/12}"/>
        </inertial>
    </xacro:macro>

    <!-- Box dimensions approach:
         X axis is facing towards observer
         Y axis is in the horizontal plane with X axis
         Z axis is vertical
         Length is dimension along Y axis
         Width is along X axis
         Height is along Z axis -->
    <xacro:macro name="box_geometry" params="width length height">
        <geometry>
            <box size="${width} ${length} ${height}"/>
        </geometry>
    </xacro:macro>

    <xacro:macro name="box_collision" params="width length height bit:=0xffff">
        <collision>
            <xacro:box_geometry width="${width}" length="${length}" height="${height}"/>
            <xacro:bitwise bit="${bit}"/>
        </collision>
    </xacro:macro>

    <xacro:macro name="cube_geometry" params="size">
        <xacro:box_geometry width="${size}" length="${size}" height="${size}"/>
    </xacro:macro>

    <xacro:macro name="cube_collision" params="size bit:=0xffff">
	    <xacro:box_collision width="${size}" length="${size}" height="${size}" bit="${bit}"/>
    </xacro:macro>

    <xacro:macro name="cube_inertia" params="mass size x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0">
        <xacro:box_inertia mass="${mass}" width="${size}" length="${size}" height="${size}" x="${x}" y="${y}" z="${z}"
                           roll="${roll}" pitch="${pitch}" yaw="${yaw}"/>
    </xacro:macro>

    <xacro:macro name="bitwise" params="bit">
        <surface>
            <contact>
                <collide_bitmask>"${bit}"</collide_bitmask>
            </contact>
        </surface>
    </xacro:macro>

</robot>
