void Curve::Evaluate( GraphDirection direction )
{
    uint32_t controlCount = 0;
    V_Vector3 points;
    {
        OS_HierarchyNodeDumbPtr::Iterator childItr = GetChildren().Begin();
        OS_HierarchyNodeDumbPtr::Iterator childEnd = GetChildren().End();
        for ( ; childItr != childEnd; ++childItr )
        {
            CurveControlPoint* point = Reflect::SafeCast< CurveControlPoint >( *childItr );
            if ( point )
            {
                points.push_back( point->GetPosition() );
                ++controlCount;
            }
        }
    }

    if ( controlCount < 4  || m_Type == CurveType::Linear ) 
    {     
        m_Points = points;
    }
    else if ( m_Type == CurveType::BSpline )
    {
        CurveGenerator::ComputeCurve( points, m_Resolution, m_Closed, CurveGenerator::kBSpline, m_Points ); 
    }
    else if ( m_Type == CurveType::CatmullRom )
    {
        CurveGenerator::ComputeCurve( points, m_Resolution, m_Closed, CurveGenerator::kCatmullRom, m_Points ); 
    }
    else
    {
        //whoa, did we get a bum curve type?
        HELIUM_BREAK();
    }


    //
    // Update buffer
    //

    if ( controlCount > 0 ) 
    {
        controlCount++;
    }

    uint32_t pointCount = (uint32_t)m_Points.size();
    if ( pointCount > 0 ) 
    {
        pointCount++;
    }

    m_Vertices->SetElementCount( controlCount + pointCount );
    m_Vertices->Update();

    Base::Evaluate(direction);
}

void CreateTool::GenerateInstanceOffsets( PlacementStyle style, float radius, float instanceRadius, V_Vector3& positions )
{
	switch ( style )
	{
	case PlacementStyles::Grid:
		{
			float radiusSquared = radius * radius;
			int numInstances = MAX( 2, (int) sqrt( radiusSquared / ( instanceRadius * instanceRadius ) ) );
			float delta = radius * 2.0f / numInstances;

			for ( float x = -radius; x <= radius; x += delta )
			{
				for ( float y = -radius; y <= radius; y += delta )
				{
					if ( x * x + y * y < radiusSquared )
					{
						Vector3 v = ( Editor::UpVector * x ) + ( Editor::OutVector * y );
						positions.push_back( v );
					}
				}
			}
			break;
		}

	case PlacementStyles::Radial:
	default:
		{
			float currentRadius = 0.0f;
			while ( currentRadius < radius )
			{
				float circumference = static_cast< float32_t >( HELIUM_TWOPI ) * currentRadius;
				int numInstances = MAX( 1, (int) ( circumference / ( 2.0f * instanceRadius ) ) );

				float deltaAngle = static_cast< float32_t >( HELIUM_TWOPI ) / numInstances;
				float currentAngle = static_cast< float32_t >( HELIUM_TWOPI ) * rand() / ( (float) RAND_MAX + 1.0f );

				for ( int i = 0; i < numInstances; ++i )
				{
					float x = currentRadius * cos( currentAngle );
					float y = currentRadius * sin( currentAngle );
					Vector3 v = ( Editor::UpVector * x ) + ( Editor::OutVector * y );
					positions.push_back( v );

					currentAngle += deltaAngle;
					while ( currentAngle > HELIUM_TWOPI )
					{
						currentAngle -= static_cast< float32_t >( HELIUM_TWOPI );
					}
				}

				currentRadius += instanceRadius + instanceRadius;
			}
			break;
		}
	}
}

void CreateTool::SetupInstanceOffsets( float instanceRadius, V_Vector3& instanceOffsets )
{
	instanceOffsets.clear();
	instanceOffsets.reserve( 256 );

	float adjustedInstanceRadius = instanceRadius / s_PaintDensity;

	GenerateInstanceOffsets( s_PaintPlacementStyle, s_PaintRadius, adjustedInstanceRadius, instanceOffsets );
	SelectInstanceOffsets( s_PaintDistributionStyle, s_PaintRadius, instanceOffsets );
	JitterInstanceOffsets( instanceRadius, s_PaintJitter, instanceOffsets );
	RandomizeInstanceOffsets( instanceOffsets );
}

float32_t Curve::CalculateCurveLength() const
{
    const Matrix4& globalTransform = this->GetGlobalTransform();
    V_Vector3 points = m_Points;
    float32_t curveLength = 0.f;
    for ( uint32_t i = 1; i < points.size() ; ++i )
    {
        Vector3 endPoint = points[i];
        Vector3 startPoint = points[i-1];
        globalTransform.TransformVertex( endPoint );
        globalTransform.TransformVertex( startPoint );
        curveLength += (endPoint - startPoint).Length();
    }
    return curveLength;
}

void CreateTool::JitterInstanceOffsets( float instanceRadius, float maxJitter, V_Vector3& offsets )
{
	V_Vector3 jitterVectors;
	jitterVectors.push_back( Editor::UpVector );
	jitterVectors.push_back( Editor::OutVector );

	V_Vector3::iterator itr = offsets.begin();
	V_Vector3::iterator end = offsets.end();
	for ( ; itr != end; ++itr )
	{
		V_Vector3::const_iterator jitterItr = jitterVectors.begin();
		V_Vector3::const_iterator jitterEnd = jitterVectors.end();
		for ( ; jitterItr != jitterEnd; ++jitterItr )
		{
			int searchTries = 10;
			while ( searchTries > 0 )
			{
				--searchTries;
				float jitter = ( rand() / ( (float) RAND_MAX + 1.0f ) ) * 2.0f - 1.0f;
				float randomNumber = rand() / ( (float) RAND_MAX + 1.0f );
				float testNumber = GetNormalProbabilityFromPercent( jitter );
				if ( randomNumber <= testNumber )
				{
					(*itr) += (*jitterItr) * jitter * maxJitter;
					searchTries = 0;
				}
			}
		}
	}
}

void AlignedBox::Transform(const Matrix4& matrix)
{
    // get the currents sample bounds
    V_Vector3 vertices;
    GetVertices( vertices );

    // reseed this box
    Reset();

    // iterate and resample the bounds
    V_Vector3::iterator itr = vertices.begin();
    V_Vector3::iterator end = vertices.end();
    for ( ; itr != end; ++itr )
    {
        // transform the sample
        matrix.TransformVertex( *itr );

        // test the sample
        Test( *itr );
    }
}

void OctreeVisualizer::incomingOctreeCallback()
{
  scan_utils::Octree<char> octree(0, 0, 0, 0, 0, 0, 1, 0);
  octree.setFromMsg(octree_message_);

  std::list<scan_utils::Triangle> triangles;
  octree.getAllTriangles(triangles);

  V_Vector3 vertices;
  V_Vector3 normals;

  vertices.resize( triangles.size() * 3 );
  normals.resize( triangles.size() );
  size_t vertexIndex = 0;
  size_t normalIndex = 0;
  std::list<scan_utils::Triangle>::iterator it = triangles.begin();
  std::list<scan_utils::Triangle>::iterator end = triangles.end();
  for ( ; it != end; it++ )
  {
    Ogre::Vector3& v1 = vertices[vertexIndex++];
    Ogre::Vector3& v2 = vertices[vertexIndex++];
    Ogre::Vector3& v3 = vertices[vertexIndex++];
    Ogre::Vector3& n = normals[normalIndex++];

    v1 = Ogre::Vector3( it->p1.x, it->p1.y, it->p1.z );
    v2 = Ogre::Vector3( it->p2.x, it->p2.y, it->p2.z );
    v3 = Ogre::Vector3( it->p3.x, it->p3.y, it->p3.z );
    robotToOgre(v1);
    robotToOgre(v2);
    robotToOgre(v3);

    n = ( v2 - v1 ).crossProduct( v3 - v1 );
    n.normalise();
  }

  triangles_mutex_.lock();

  vertices_.clear();
  normals_.clear();

  vertices.swap( vertices_ );
  normals.swap( normals_ );

  new_message_ = true;
  triangles_mutex_.unlock();
}

void CreateTool::SelectInstanceOffsets( DistributionStyle style, float radius, V_Vector3& offsets )
{
	V_Vector3 selectedOffsets;
	selectedOffsets.reserve( offsets.size() );

	V_Vector3::iterator itr = offsets.begin();
	V_Vector3::iterator end = offsets.end();
	for ( ; itr != end; ++itr )
	{
		switch ( style )
		{
		case DistributionStyles::Uniform:
			{
				float randomNumber = rand() / ( (float) RAND_MAX + 1.0f );
				if ( randomNumber <= 0.5f )
				{
					selectedOffsets.push_back( *itr );
				}
				break;
			}

		case DistributionStyles::Linear:
			{
				float radiusPercent = (*itr).Length() / radius;
				float randomNumber = rand() / ( (float) RAND_MAX + 1.0f );
				float testNumber = 1.0f - radiusPercent;
				if ( randomNumber <= testNumber )
				{
					selectedOffsets.push_back( *itr );
				}
				break;
			}

		case DistributionStyles::Normal:
			{
				float radiusPercent = (*itr).Length() / radius;
				float randomNumber = rand() / ( (float) RAND_MAX + 1.0f );
				float testNumber = GetNormalProbabilityFromPercent( radiusPercent );
				if ( randomNumber <= testNumber )
				{
					selectedOffsets.push_back( *itr );
				}
				break;
			}

		case DistributionStyles::Constant:
		default:
			selectedOffsets.push_back( *itr );
			break;
		}
	}

	offsets.clear();
	offsets.reserve( selectedOffsets.size() );
	itr = selectedOffsets.begin();
	end = selectedOffsets.end();
	for ( ; itr != end; ++itr )
	{
		offsets.push_back( *itr );
	}
}

void CreateTool::RandomizeInstanceOffsets( V_Vector3& offsets )
{
	V_Vector3 newOffsets;
	newOffsets.reserve( offsets.size() );

	while ( offsets.size() )
	{
		V_Vector3::iterator itr = offsets.begin() + ( rand() % offsets.size() );
		newOffsets.push_back( *itr );
		offsets.erase( itr );
	}

	V_Vector3::iterator itr = newOffsets.begin();
	V_Vector3::iterator end = newOffsets.end();
	for ( ; itr != end; ++itr )
	{
		offsets.push_back( *itr );
	}
}