#include <assert.h>
#include "primitive.h"
#include "relativepositionsensor.h"
#include "mathutils.h"