vec3 operator+(vec3 left, const vec3& right)
		{
			return left.add(right);
		}

void ToFloatColour(const vec3<U8>& byteColour, vec3<F32>& colourOut) noexcept {
    colourOut.set(CHAR_TO_FLOAT_SNORM(byteColour.r),
                  CHAR_TO_FLOAT_SNORM(byteColour.g),
                  CHAR_TO_FLOAT_SNORM(byteColour.b));
}

void ToByteColour(const vec3<F32>& floatColour, vec3<U8>& colourOut) noexcept {
    colourOut.set(FLOAT_TO_CHAR_SNORM(floatColour.r),
                  FLOAT_TO_CHAR_SNORM(floatColour.g),
                  FLOAT_TO_CHAR_SNORM(floatColour.b));
}

int main()
{
  
  // Test: Cubic Bezier Curve
  {
    const vec3 input[] = { 
      vec3(0.0f), 
      vec3(1.0f, 10.0f, 0.0f), 
      vec3(2.0f, -10.0f, 0.0f), 
      vec3(3.0f, 0.0f, 0.0f) 
    };
    
    for(F32 t = 0.0f; t<=1.0f; t+=0.1f){
      const vec3 vt = bezier_curve(4, input, t);
      const vec3 vd = bezier_curve_der(4, input, t);
      
      if(!_test(vt.getZ(), 0.0f))
        return 1;
      if(!_test(vt.getX(), 3.0f*t))
        return 1;
      if(!_test(vt.getY(), 3.0f*t*(1-t)*(1-2.0f*t)*10.0f))
        return 1;
      if(!_test(vd.getX(), 3.0f))
        return 1;
      if(!_test(vd.getZ(), 0.0f))
        return 1;
      if(!_test(vd.getY(), 30.0f*(1.0f-6.0f*t+6.0f*t*t)))
        return 1;
    }
  }
  
  // Test: Cubic Bezier Patch
  {
    const vec3 input[4][3] = {
      { 
        vec3(0.0f, 0.0f, 0.0f), 
        vec3(0.0f, 1.0f, 0.0f), 
        vec3(0.0f, 2.0f, 0.0f)
      },
      { 
        vec3(1.0f, 0.0f, 0.0f), 
        vec3(1.0f, 1.0f, 0.0f), 
        vec3(1.0f, 2.0f, 0.0f)
      },
      {
        vec3(2.0f, 0.0f, 0.0f), 
        vec3(2.0f, 1.0f, 0.0f), 
        vec3(2.0f, 2.0f, 0.0f)
      },
      {
        vec3(3.0f, 0.0f, 0.0f), 
        vec3(3.0f, 1.0f, 0.0f), 
        vec3(3.0f, 2.0f, 0.0f)
      }
    };
    
    for(F32 u = 0.0f; u<=1.0f; u+=0.1f)
      for(F32 v = 0.0f; v<=1.0f; v+=0.1f){
        const vec3 vt = bezier_patch(4, 3, (vec3*)input, u, v);
        const vec3 vdv = bezier_patch_dv(4, 3, (vec3*)input, u, v);
        const vec3 vdu = bezier_patch_du(4, 3, (vec3*)input, u, v);
        const vec3 normal = normalize(cross(vdu, vdv));
        
        if(!_test(vt.getZ(), 0.0f))
          return 1;
        if(!_test(vt.getX(), u*3.0f))
          return 1;
        if(!_test(vt.getY(), v*2.0f))
          return 1;
        if(!_test(vdv, vec3(0.0f, 2.0f, 0.0f)))
          return 1;
        if(!_test(vdu, vec3(3.0f, 0.0f, 0.0f)))
          return 1;
        if(!_test(normal, vec3(0.0f, 0.0f, 1.0f)))
          return 1;
      }
    
  }

  // 

  return 0;
}

	bool vec3::operator==(const vec3 & b)const {
		return magnitude() == b.magnitude();
	}

 plane3(vec3<T> &v0, vec3<T> &norm): n(norm), v0(v0) {
     n.normalize();
 }

void
SceneIdeasPrivate::draw()
{
    viewFromSpline_.getCurrentVec(currentTime_, viewFrom_);
    viewToSpline_.getCurrentVec(currentTime_, viewTo_);
    lightPosSpline_.getCurrentVec(currentTime_, lightPos_);
    logoPosSpline_.getCurrentVec(currentTime_, logoPos_);
    logoRotSpline_.getCurrentVec(currentTime_, logoRot_);

    // Tell the logo its new position
    logo_.setPosition(logoPos_);

    vec4 lp4(lightPos_.x(), lightPos_.y(), lightPos_.z(), 0.0);

    //
    // SHADOW
    //
    modelview_.loadIdentity();
    modelview_.lookAt(viewFrom_.x(), viewFrom_.y(), viewFrom_.z(), 
                      viewTo_.x(), viewTo_.y(), viewTo_.z(),
                      0.0, 1.0, 0.0);

    float pca(0.0);
    if (viewFrom_.y() > 0.0)
    {
        table_.draw(modelview_, projection_, lightPos_, logoPos_, currentTime_, pca);
    }

    glEnable(GL_CULL_FACE); 
    glDisable(GL_DEPTH_TEST); 

    if (logoPos_.y() < 0.0)
    {
        // Set the color assuming we're still under the table.
        uvec3 flatColor(128 / 2,  102 / 2,  179 / 2);
        if (logoPos_.y() > -0.33)
        {
            // We're emerging from the table
            float c(1.0 - logoPos_.y() / -0.33);
            pca /= 4.0;
            flatColor.x(static_cast<unsigned int>(128.0 * (1.0 - c) * 0.5 + 255.0 * pca * c));
            flatColor.y(static_cast<unsigned int>(102.0 * (1.0 - c) * 0.5 + 255.0 * pca * c));
            flatColor.z(static_cast<unsigned int>(179.0 * (1.0 - c) * 0.5 + 200.0 * pca * c));
        }

        modelview_.push();
        modelview_.scale(0.04, 0.0, 0.04);
        modelview_.rotate(-90.0, 1.0, 0.0, 0.0);
        modelview_.rotate(0.1 * static_cast<int>(10.0 * logoRot_.z()), 0.0, 0.0, 1.0);
        modelview_.rotate(0.1 * static_cast<int>(10.0 * logoRot_.y()), 0.0, 1.0, 0.0);
        modelview_.rotate(0.1 * static_cast<int>(10.0 * logoRot_.x()), 1.0, 0.0, 0.0);
        modelview_.rotate(0.1 * 353, 1.0, 0.0, 0.0);
        modelview_.rotate(0.1 * 450, 0.0, 1.0, 0.0);

        logo_.draw(modelview_, projection_, lightPositions_[0], SGILogo::LOGO_FLAT, flatColor);

        modelview_.pop();
    }
    
    if (logoPos_.y() > 0.0)
    {
        modelview_.push();
        modelview_.translate(lightPos_.x(), lightPos_.y(), lightPos_.z());
        mat4 tv;
        tv[3][1] = -1.0;
        tv[3][3] = 0.0;
        tv[0][0] = tv[1][1] = tv[2][2] = lightPos_.y();
        modelview_ *= tv;
        modelview_.translate(-lightPos_.x() + logoPos_.x(),
                             -lightPos_.y() + logoPos_.y(),
                             -lightPos_.z() + logoPos_.z());
        modelview_.scale(0.04, 0.04, 0.04);
        modelview_.rotate(-90.0, 1.0, 0.0, 0.0);
        modelview_.rotate(0.1 * static_cast<int>(10.0 * logoRot_.z()), 0.0, 0.0, 1.0);
        modelview_.rotate(0.1 * static_cast<int>(10.0 * logoRot_.y()), 0.0, 1.0, 0.0);
        modelview_.rotate(0.1 * static_cast<int>(10.0 * logoRot_.x()), 1.0, 0.0, 0.0);
        modelview_.rotate(35.3, 1.0, 0.0, 0.0);
        modelview_.rotate(45.0, 0.0, 1.0, 0.0);

        logo_.draw(modelview_, projection_, lightPositions_[0], SGILogo::LOGO_SHADOW);

        modelview_.pop();
    }
    //
    // DONE SHADOW 
    //

    glEnable(GL_DEPTH_TEST);
    glDisable(GL_CULL_FACE);

    modelview_.loadIdentity();
    modelview_.lookAt(viewFrom_.x(), viewFrom_.y(), viewFrom_.z(),
                      viewTo_.x(), viewTo_.y(), viewTo_.z(), 
                      0.0, 1.0, 0.0);
    modelview_.push();
    modelview_.translate(lightPos_.x(), lightPos_.y(), lightPos_.z());
    modelview_.scale(0.1, 0.1, 0.1);
    float x(lightPos_.x() - logoPos_.x());
    float y(lightPos_.y() - logoPos_.y());
    float z(lightPos_.z() - logoPos_.z());
//.........这里部分代码省略.........

    /** Updates the AABB to contain the given point. */
    void addPoint(const vec3& p) 
    {
      if (isNull())
      {
        mMax = p;
        mMin = p;
        return;
      }

      if ( mMax.x() < p.x() ) mMax.x() = p.x();
      if ( mMax.y() < p.y() ) mMax.y() = p.y();
      if ( mMax.z() < p.z() ) mMax.z() = p.z();
      if ( mMin.x() > p.x() ) mMin.x() = p.x();
      if ( mMin.y() > p.y() ) mMin.y() = p.y();
      if ( mMin.z() > p.z() ) mMin.z() = p.z();
    }

  void graph::reset(math::natural n) {

    width = n;
    height = n;
    
    obj.resize( width * height);
    constraint.mesh = mesh_type( width * height );

    auto pos = [&](natural i) -> vec3& {
      return obj[i].conf;
    };

    auto vel = [&](natural i) -> vec3& {
      return obj[i].velocity;
    };

  

    auto edge = [&](real rest) -> edge_type {
      edge_type res;
      res.rest = rest;
      res.constraint = new phys::constraint::bilateral(1);
      return res;
    };

    // place vertices
    for(natural i = 0 ; i < n * n; ++i ) {
      const natural x = i / width;
      const natural y = i % width;
      
      pos(i) = (sceneRadius()/2) * vec3(y, 0, x) / width;
      vel(i).setZero();
    }

    // create edges
    for(natural i = 0 ; i < n * n; ++i ) {
      const natural x = i / width;
      const natural y = i % width;
      
      bool ok_height = (x < height - 1);
      bool ok_width = (y < width - 1);
      
      if( ok_height ) {
	real rest = (pos(i) - pos(i + width)).norm();
	
	boost::add_edge(i, i + width, edge(rest), constraint.mesh);
      }

      if( ok_width ) { 
	real rest = (pos(i) - pos(i + 1)).norm();
	boost::add_edge(i, i + 1, edge(rest), constraint.mesh);
      }
      
      if( ok_height && ok_width ) {
	// real rest = (pos(i) - pos(i + width + 1)).norm();
	// boost::add_edge(i, i + width + 1, edge(rest), constraint.mesh);

	// rest = (pos(i + 1) - pos(i + width)).norm();
	// boost::add_edge(i + 1, i + width, edge(rest), constraint.mesh);
      }

    }

    frame[0]->transform( SE3::translation( pos(0) ) );
    frame[1]->transform( SE3::translation( pos(width - 1) ) );

    system.clear();
    
    // masses
    core::each( obj, [&](const obj_type& o) {
	system.mass[ &o ].setIdentity(3, 3);
	system.resp[ &o ].setIdentity(3, 3);
      });
    
    constraint.lambda.clear();
    

    constraint.update.clear();

    // setup constraints
    core::each( boost::edges(constraint.mesh), [&](const mesh_type::edge_descriptor& e) {
	auto c = constraint.mesh[e].constraint;

	natural i = boost::source(e, constraint.mesh);
	natural j = boost::target(e, constraint.mesh);
	
	const real rest = constraint.mesh[e].rest;
	
	constraint.update[c] = [&,i,j,c,e,rest] {
	  
	  const vec3 diff = obj[i].conf - obj[j].conf; 
	  const vec3 n = diff.normalized();
	  
	  auto& row = system.constraint.bilateral.matrix[ c ];
      
	  row[ &obj[i] ] = n.transpose();
	  row[ &obj[j] ] = -n.transpose();
      
	  const real gamma = 1;
  
//.........这里部分代码省略.........

void JetEngineEmitter::EmitFromTag(const CppContextObject* object, const uitbc::ChunkyClass::Tag& tag, float frame_time) {
	bool particles_enabled;
	v_get(particles_enabled, =, UiCure::GetSettings(), kRtvarUi3DEnableparticles, false);
	if (!particles_enabled) {
		return;
	}

	enum FloatValue {
		kFvStartR = 0,
		kFvStartG,
		kFvStartB,
		kFvEndR,
		kFvEndG,
		kFvEndB,
		kFvX,
		kFvY,
		kFvZ,
		kFvRadiusX,
		kFvRadiusY,
		kFvRadiusZ,
		kFvScaleX,
		kFvScaleY,
		kFvScaleZ,
		kFvDirectionX,
		kFvDirectionY,
		kFvDirectionZ,
		kFvDensity,
		kFvOpacity,
		kFvOvershootOpacity,
		kFvOvershootCutoffDot,
		kFvOvershootDistanceUpscale,
		kFvOvershootEngineFactorBase,
		kFvCount
	};
	if (tag.float_value_list_.size() != kFvCount ||
		tag.string_value_list_.size() != 0 ||
		tag.body_index_list_.size() != 0 ||
		tag.engine_index_list_.size() != 1 ||
		tag.mesh_index_list_.size() < 1) {
		log_.Errorf("The fire tag '%s' has the wrong # of parameters.", tag.tag_name_.c_str());
		deb_assert(false);
		return;
	}
	const int engine_index = tag.engine_index_list_[0];
	if (engine_index >= object->GetPhysics()->GetEngineCount()) {
		return;
	}
	const tbc::PhysicsEngine* engine = object->GetPhysics()->GetEngine(engine_index);
	const float throttle_up_speed = Math::GetIterateLerpTime(tag.float_value_list_[kFvOvershootEngineFactorBase]*0.5f, frame_time);
	const float throttle_down_speed = Math::GetIterateLerpTime(tag.float_value_list_[kFvOvershootEngineFactorBase], frame_time);
	const float engine_throttle = engine->GetLerpThrottle(throttle_up_speed, throttle_down_speed, true);
	const quat orientation = object->GetOrientation();
	vec3 _radius(tag.float_value_list_[kFvRadiusX], tag.float_value_list_[kFvRadiusY], tag.float_value_list_[kFvRadiusZ]);
	_radius.x *= Math::Lerp(1.0f, tag.float_value_list_[kFvScaleX], engine_throttle);
	_radius.y *= Math::Lerp(1.0f, tag.float_value_list_[kFvScaleY], engine_throttle);
	_radius.z *= Math::Lerp(1.0f, tag.float_value_list_[kFvScaleZ], engine_throttle);
	vec3 _position(tag.float_value_list_[kFvX], tag.float_value_list_[kFvY], tag.float_value_list_[kFvZ]);
	_position = orientation * _position;
	const vec3 _color(tag.float_value_list_[kFvEndR], tag.float_value_list_[kFvEndB], tag.float_value_list_[kFvEndB]);

	bool create_particle = false;
	const float density = tag.float_value_list_[kFvDensity];
	float exhaust_intensity;
	v_get(exhaust_intensity, =(float), UiCure::GetSettings(), kRtvarUi3DExhaustintensity, 1.0);
	interleave_timeout_ -= Math::Lerp(0.3f, 1.0f, engine_throttle) * exhaust_intensity * frame_time;
	if (interleave_timeout_ <= 0) {	// Release particle this frame?
		create_particle = true;
		interleave_timeout_ = 0.05f / density;
	} else {
		create_particle = false;
	}

	const float dx = tag.float_value_list_[kFvRadiusX];
	const float dy = tag.float_value_list_[kFvRadiusY];
	const float dz = tag.float_value_list_[kFvRadiusZ];
	const vec3 start_color(tag.float_value_list_[kFvStartR], tag.float_value_list_[kFvStartB], tag.float_value_list_[kFvStartB]);
	const float _opacity = tag.float_value_list_[kFvOpacity];
	const vec3 direction = orientation * vec3(tag.float_value_list_[kFvDirectionX], tag.float_value_list_[kFvDirectionY], tag.float_value_list_[kFvDirectionZ]);
	const vec3 velocity = direction + object->GetVelocity();
	uitbc::ParticleRenderer* particle_renderer = (uitbc::ParticleRenderer*)ui_manager_->GetRenderer()->GetDynamicRenderer("particle");
	const float particle_time = density;
	float particle_size;	// Pick second size.
	if (dx > dy && dy > dz) {
		particle_size = dy;
	} else if (dy > dx && dx > dz) {
		particle_size = dx;
	} else {
		particle_size = dz;
	}
	particle_size *= 0.2f;

	const float _distance_scale_factor = tag.float_value_list_[kFvOvershootDistanceUpscale];
	for (size_t y = 0; y < tag.mesh_index_list_.size(); ++y) {
		tbc::GeometryBase* mesh = object->GetMesh(tag.mesh_index_list_[y]);
		if (mesh) {
			int phys_index = -1;
			str mesh_name;
			xform transform;
			float mesh_scale;
			((uitbc::ChunkyClass*)object->GetClass())->GetMesh(tag.mesh_index_list_[y], phys_index, mesh_name, transform, mesh_scale);
//.........这里部分代码省略.........

void ewol::compositing::Drawing::setClipping(const vec3& _pos, const vec3& _posEnd) {
	// note the internal system all time request to have a bounding all time in the same order
	if (_pos.x() <= _posEnd.x()) {
		m_clippingPosStart.setX(_pos.x());
		m_clippingPosStop.setX(_posEnd.x());
	} else {
		m_clippingPosStart.setX(_posEnd.x());
		m_clippingPosStop.setX(_pos.x());
	}
	if (_pos.y() <= _posEnd.y()) {
		m_clippingPosStart.setY(_pos.y());
		m_clippingPosStop.setY(_posEnd.y());
	} else {
		m_clippingPosStart.setY(_posEnd.y());
		m_clippingPosStop.setY(_pos.y());
	}
	if (_pos.z() <= _posEnd.z()) {
		m_clippingPosStart.setZ(_pos.z());
		m_clippingPosStop.setZ(_posEnd.z());
	} else {
		m_clippingPosStart.setZ(_posEnd.z());
		m_clippingPosStop.setZ(_pos.z());
	}
	m_clippingEnable = true;
}

void ewol::compositing::Drawing::lineTo(const vec3& _dest) {
	resetCount();
	internalSetColor(m_color);
	//EWOL_VERBOSE("DrawLine : " << m_position << " to " << _dest);
	if (m_position.x() == _dest.x() && m_position.y() == _dest.y() && m_position.z() == _dest.z()) {
		//EWOL_WARNING("Try to draw a line width 0");
		return;
	}
	//teta = tan-1(oposer/adjacent)
	float teta = 0;
	if (m_position.x() <= _dest.x()) {
		teta = atan((_dest.y()-m_position.y())/(_dest.x()-m_position.x()));
	} else {
		teta = M_PI + atan((_dest.y()-m_position.y())/(_dest.x()-m_position.x()));
	}
	if (teta < 0) {
		teta += 2*M_PI;
	} else if (teta > 2*M_PI) {
		teta -= 2*M_PI;
	}
	//EWOL_DEBUG("teta = " << (teta*180/(M_PI)) << " deg." );
	float offsety = sin(teta-M_PI/2) * (m_thickness/2);
	float offsetx = cos(teta-M_PI/2) * (m_thickness/2);
	setPoint(vec3(m_position.x() - offsetx, m_position.y() - offsety, m_position.z()) );
	setPoint(vec3(m_position.x() + offsetx, m_position.y() + offsety, m_position.z()) );
	setPoint(vec3(_dest.x()      + offsetx, _dest.y()      + offsety, m_position.z()) );
	
	setPoint(vec3(_dest.x()      + offsetx, _dest.y()      + offsety, _dest.z()) );
	setPoint(vec3(_dest.x()      - offsetx, _dest.y()      - offsety, _dest.z()) );
	setPoint(vec3(m_position.x() - offsetx, m_position.y() - offsety, _dest.z()) );
	// update the system position :
	m_position = _dest;
}

 inline T cos(const vec3& other) {
     return *this * other / norm() / other.norm();
 }

float DistanceToAABBSqr(const vec3& p, const AABB& aabb)
{
	if (p.x < aabb.m_mins.x)
	{
		if (p.y < aabb.m_mins.y)
		{
			if (p.z < aabb.m_mins.z)
				return p.DistanceSqr(aabb.m_mins);
			else if (p.z > aabb.m_maxs.z)
				return p.DistanceSqr(vec3(aabb.m_mins.x, aabb.m_mins.y, aabb.m_maxs.z));
			else
				return DistanceToLineSqr(p, aabb.m_mins, vec3(aabb.m_mins.x, aabb.m_mins.y, aabb.m_maxs.z));
		}
		else if (p.y > aabb.m_maxs.y)
		{
			if (p.z < aabb.m_mins.z)
				return p.DistanceSqr(vec3(aabb.m_mins.x, aabb.m_maxs.y, aabb.m_mins.z));
			else if (p.z > aabb.m_maxs.z)
				return p.DistanceSqr(vec3(aabb.m_mins.x, aabb.m_maxs.y, aabb.m_maxs.z));
			else
				return DistanceToLineSqr(p, vec3(aabb.m_mins.x, aabb.m_maxs.y, aabb.m_mins.z), vec3(aabb.m_mins.x, aabb.m_maxs.y, aabb.m_maxs.z));
		}
		else
		{
			if (p.z < aabb.m_mins.z)
				return DistanceToLineSqr(p, aabb.m_mins, vec3(aabb.m_mins.x, aabb.m_maxs.y, aabb.m_mins.z));
			else if (p.z > aabb.m_maxs.z)
				return DistanceToLineSqr(p, vec3(aabb.m_mins.x, aabb.m_mins.y, aabb.m_maxs.z), vec3(aabb.m_mins.x, aabb.m_maxs.y, aabb.m_maxs.z));
			else
				return DistanceToPlaneSqr(p, aabb.m_mins, vec3(-1, 0, 0));
		}
	}
	else if (p.x > aabb.m_maxs.x)
	{
		if (p.y < aabb.m_mins.y)
		{
			if (p.z < aabb.m_mins.z)
				return p.DistanceSqr(vec3(aabb.m_maxs.x, aabb.m_mins.y, aabb.m_mins.z));
			else if (p.z > aabb.m_maxs.z)
				return p.DistanceSqr(vec3(aabb.m_maxs.x, aabb.m_mins.y, aabb.m_maxs.z));
			else
				return DistanceToLineSqr(p, vec3(aabb.m_maxs.x, aabb.m_mins.y, aabb.m_mins.z), vec3(aabb.m_maxs.x, aabb.m_mins.y, aabb.m_maxs.z));
		}
		else if (p.y > aabb.m_maxs.y)
		{
			if (p.z < aabb.m_mins.z)
				return p.DistanceSqr(vec3(aabb.m_maxs.x, aabb.m_maxs.y, aabb.m_mins.z));
			else if (p.z > aabb.m_maxs.z)
				return p.DistanceSqr(vec3(aabb.m_maxs.x, aabb.m_maxs.y, aabb.m_maxs.z));
			else
				return DistanceToLineSqr(p, vec3(aabb.m_maxs.x, aabb.m_maxs.y, aabb.m_mins.z), vec3(aabb.m_maxs.x, aabb.m_maxs.y, aabb.m_maxs.z));
		}
		else
		{
			if (p.z < aabb.m_mins.z)
				return DistanceToLineSqr(p, vec3(aabb.m_maxs.x, aabb.m_mins.y, aabb.m_mins.z), vec3(aabb.m_maxs.x, aabb.m_maxs.y, aabb.m_mins.z));
			else if (p.z > aabb.m_maxs.z)
				return DistanceToLineSqr(p, vec3(aabb.m_maxs.x, aabb.m_mins.y, aabb.m_maxs.z), vec3(aabb.m_maxs.x, aabb.m_maxs.y, aabb.m_maxs.z));
			else
				return DistanceToPlaneSqr(p, aabb.m_maxs, vec3(1, 0, 0));
		}
	}
	else
	{
		if (p.y < aabb.m_mins.y)
		{
			if (p.z < aabb.m_mins.z)
				return DistanceToLineSqr(p, aabb.m_mins, vec3(aabb.m_maxs.x, aabb.m_mins.y, aabb.m_mins.z));
			else if (p.z > aabb.m_maxs.z)
				return DistanceToLineSqr(p, vec3(aabb.m_mins.x, aabb.m_mins.y, aabb.m_maxs.z), vec3(aabb.m_maxs.x, aabb.m_mins.y, aabb.m_maxs.z));
			else
				return DistanceToPlaneSqr(p, aabb.m_mins, vec3(0, -1, 0));
		}
		else if (p.y > aabb.m_maxs.y)
		{
			if (p.z < aabb.m_mins.z)
				return DistanceToLineSqr(p, vec3(aabb.m_mins.x, aabb.m_maxs.y, aabb.m_mins.z), vec3(aabb.m_maxs.x, aabb.m_maxs.y, aabb.m_mins.z));
			else if (p.z > aabb.m_maxs.z)
				return DistanceToLineSqr(p, vec3(aabb.m_mins.x, aabb.m_maxs.y, aabb.m_maxs.z), aabb.m_maxs);
			else
				return DistanceToPlaneSqr(p, aabb.m_maxs, vec3(0, 1, 0));
		}
		else
		{
			if (p.z < aabb.m_mins.z)
				return DistanceToPlaneSqr(p, aabb.m_mins, vec3(0, 0, -1));
			else if (p.z > aabb.m_maxs.z)
				return DistanceToPlaneSqr(p, aabb.m_maxs, vec3(0, 0, 1));
			else
				return 0;
		}
	}
}

/// Returns the normalized vector of a vector.
inline vec3 normalize(const vec3& v)
{
	return (1 / v.norm()) * v;
}

bool LineSegmentIntersectsSphere(const vec3& v1, const vec3& v2, const vec3& s, float flRadius, vec3& vecPoint, vec3& vecNormal)
{
	vec3 vecLine = v2 - v1;
	vec3 vecSphere = v1 - s;

	if (vecLine.LengthSqr() == 0)
	{
		if (vecSphere.LengthSqr() < flRadius*flRadius)
		{
			vecPoint = v1;
			vecNormal = vecSphere.Normalized();
			return true;
		}
		else
			return false;
	}

	float flA = vecLine.LengthSqr();
	float flB = 2 * vecSphere.Dot(vecLine);
	float flC1 = s.LengthSqr() + v1.LengthSqr();
	float flC2 = (s.Dot(v1)*2);
	float flC = flC1 - flC2 - flRadius*flRadius;

	float flBB4AC = flB*flB - 4*flA*flC;
	if (flBB4AC < 0)
		return false;

	float flSqrt = sqrt(flBB4AC);
	float flPlus = (-flB + flSqrt)/(2*flA);
	float flMinus = (-flB - flSqrt)/(2*flA);

	bool bPlusBelow0 = flPlus < 0;
	bool bMinusBelow0 = flMinus < 0;
	bool bPlusAbove1 = flPlus > 1;
	bool bMinusAbove1 = flMinus > 1;

	// If both are above 1 or below 0, then we're not touching the sphere.
	if (bMinusBelow0 && bPlusBelow0 || bPlusAbove1 && bMinusAbove1)
		return false;

	if (bMinusBelow0 && bPlusAbove1)
	{
		// We are inside the sphere.
		vecPoint = v1;
		vecNormal = (v1 - s).Normalized();
		return true;
	}

	if (bMinusAbove1 && bPlusBelow0)
	{
		// We are inside the sphere. Is this even possible? I dunno. I'm putting an assert here to see.
		// If it's still here later that means no.
		TAssert(false);
		vecPoint = v1;
		vecNormal = (v1 - s).Normalized();
		return true;
	}

	// If flPlus is below 1 and flMinus is below 0 that means we started our trace inside the sphere and we're heading out.
	// Don't intersect with the sphere in this case so that things on the inside can get out without getting stuck.
	if (bMinusBelow0 && !bPlusAbove1)
		return false;

	// So at this point, flMinus is between 0 and 1, and flPlus is above 1.
	// In any other case, we intersect with the sphere and we use the flMinus value as the intersection point.
	float flDistance = vecLine.Length();
	vec3 vecDirection = vecLine / flDistance;

	vecPoint = v1 + vecDirection * (flDistance * flMinus);

	// Oftentimes we are slightly stuck inside the sphere. Pull us out a little bit.
	vec3 vecDifference = vecPoint - s;
	float flDifferenceLength = vecDifference.Length();
	vecNormal = vecDifference / flDifferenceLength;
	if (flDifferenceLength < flRadius)
		vecPoint += vecNormal * ((flRadius - flDifferenceLength) + 0.00001f);
	TAssert((vecPoint - s).LengthSqr() >= flRadius*flRadius);

	return true;
}

void Cube::rotate(vec3 velocity, float elapsedTime)
{
	float angleInclinaisonPlan = 0.0f;

	vec3 norm;
	vec3 normal;
	vec3 d;
	float angle;
	float speed;

	speed = ((velocity.Length())/getSize()) * elapsedTime;// / (90);

	//normal = this->getNormal();
	normal = vec3(0,1,0);

	if(normal != this->normal)
	{
		this->normal = normal;

		norm = MF.normalizeVector(normal);

		norm = vec3(norm.x * (float)sin((angleInclinaisonPlan/M_PI)*180), norm.y * (float)cos((angleInclinaisonPlan/M_PI)*180),0);
		this->norm = norm;
	}

	d.x = velocity.z;
	d.y = 0.0;//velocity.y;
	d.z = velocity.x * -1;

	d = MF.normalizeVector(d);

	d *= -1;
	
	angle = speed * 180.0f / M_PI;

	if(angle >= this->rotationPrecision)
	{

		vec4 q;
		vec4 qPrime;
		vec4 result;
	
		vec3 u;

		u = d;

		u = MF.normalizeVector(u);

		q = this->components.getQuaternion()->getQuaternion4f(angle, u);

		qPrime = this->components.getQuaternion()->getQuaternion4f(angle, u* (-1));

		int i;

		for(i = 0; i < 8; ++i)
		{
			this->quadPosition.at(i) -= this->centerOfObject;
			result = this->components.getQuaternion()->multiplyQuaternion4f(q, vec4(0,this->quadPosition.at(i).x, this->quadPosition.at(i).y, this->quadPosition.at(i).z));
			this->quadPosition.at(i) = this->components.getQuaternion()->multiplyQuaternion3f(result, qPrime);
			this->quadPosition.at(i) += this->centerOfObject;
		}

		// AXES

		for(i = 0; i < 2; ++i)
		{
			this->axeX.at(i) -= this->centerOfObject;
			result =  this->components.getQuaternion()->multiplyQuaternion4f(q, vec4(0,axeX.at(i).x, axeX.at(i).y, axeX.at(i).z));
			this->axeX.at(i) = this->components.getQuaternion()->multiplyQuaternion3f(result, qPrime);
			this->axeX.at(i) += this->centerOfObject;

			this->axeY.at(i) -= this->centerOfObject;
			result =  this->components.getQuaternion()->multiplyQuaternion4f(q, vec4(0,axeY.at(i).x, axeY.at(i).y, axeY.at(i).z));
			this->axeY.at(i) = this->components.getQuaternion()->multiplyQuaternion3f(result, qPrime);
			this->axeY.at(i) += this->centerOfObject;;

			this->axeZ.at(i) -= this->centerOfObject;
			result =  this->components.getQuaternion()->multiplyQuaternion4f(q, vec4(0,axeZ.at(i).x, axeZ.at(i).y, axeZ.at(i).z));
			this->axeZ.at(i) = this->components.getQuaternion()->multiplyQuaternion3f(result, qPrime);
			this->axeZ.at(i) += this->centerOfObject;
		}

		getComponents()->getRigidBody()->rotate(angle, d);
	}
}

	float vec3::angle(const vec3& b)const {
		float d = dot(b);
		float m = magnitude() * b.magnitude();
		return acos(d / m) * 180.0 / PI;
	}

 ray3(vec3<T> &p0, vec3<T> &u): u(u), p0(p0) {
     u.normalize();
 }

void ToIntColour(const vec3<F32>& floatColour, vec3<I32>& colourOut) noexcept {
    colourOut.set(to_U32(FLOAT_TO_SCHAR_SNORM(floatColour.r)),
                  to_U32(FLOAT_TO_SCHAR_SNORM(floatColour.g)),
                  to_U32(FLOAT_TO_SCHAR_SNORM(floatColour.b)));
}