SolidBox * Model::createbox(glm::mat4 mat)
{
    auto solid = new SolidBox();
    auto meshs = file->getMeshes();

    for (auto i = 0; i < meshs.size(); i++)
    {
        for (auto& vb : file->getVertices())
        {
            auto v = glm::vec3(mat * glm::vec4(vb, 1.0));

            if (v.x < min.x)	min.x = v.x;
            if (v.y < min.y)	min.y = v.y;
            if (v.z < min.z)	min.z = v.z;
            if (v.x > max.x)	max.x = v.x;
            if (v.y > max.y)	max.y = v.y;
            if (v.z > max.z)	max.z = v.z;
        }
    }

    
    solid->setPositionVector(glm::vec3(min));
    solid->setSizeVector(glm::vec3(max));

    min = Vertex(1000000, 1000000, 1000000);
    max = Vertex(-1000000, -1000000, -1000000);

    return solid;
}
SolidCylinder * Model::createCylinder(glm::mat4 mat, double)
{
    auto solid = new SolidCylinder();
    auto meshs = file->getMeshes();

    for (auto i = 0; i < meshs.size(); i++)
    {
        for (auto& vb : file->getVertices())
        {
            auto v = glm::vec3(mat * glm::vec4(vb, 1.0));

            if (v.x < min.x)	min.x = v.x;
            if (v.y < min.y)	min.y = v.y;
            if (v.z < min.z)	min.z = v.z;
            if (v.x > max.x)	max.x = v.x;
            if (v.y > max.y)	max.y = v.y;
            if (v.z > max.z)	max.z = v.z;
        }
    }

    auto center = (max - min) / glm::vec3(2, 2, 2);
    
    double radius = glm::length(max - center);
    
    solid->setPositionVector(center);
    solid->setRadius(radius);

    return solid;
}
void HOMERServoController::setDefaultState()
{
  setPositionVector(defaultPosition);
  setTimeVector(defaultTime);
  flush();
}