void WindowAnotationManager::on_actionWeights4_triggered()
{
    VectorF weights;
    weights.push_back(1.0); weights.push_back(0.5); weights.push_back(0.25);  weights.push_back(0.125);
    tracker->weights = weights;
    updateTrackerInfo();
}

void LineArtist::drawLine(f32 x1, f32 y1, f32 x2, f32 y2, const Color& color)
{
	sf::Vertex vtxList[4];

	VectorF begin = { x1, y1 };
	VectorF end = { x2, y2 };

	VectorF direction = end - begin;
	direction.normalize();

	VectorF perpendicularVec = { -direction.y, direction.x };

	VectorF offset = (m_thickness / 2.0f) * perpendicularVec;

	VectorF tempVtxList[4] = {
		{ begin + offset },
		{ end + offset },
		{ end - offset },
		{ begin - offset }
	};

	const sf::Color sfColor = sf::Color(color.getRgba());

	for (i32 i = 0; i < 4; ++i)
	{
		vtxList[i].position = {
			tempVtxList[i].x, tempVtxList[i].y
		};
		vtxList[i].color = sfColor;
	}

	m_sharedWin.getObject()->draw(vtxList, 4, sf::Quads, m_renderStates);
}

void VertexIsotropicOffsetParameter::apply(VectorF& results,
        const PatternParameter::Variables& vars) {
    const size_t dim = m_wire_network->get_dim();
    const size_t num_vertices = m_wire_network->get_num_vertices();
    const size_t roi_size = m_roi.size();
    const VectorF center = m_wire_network->center();
    const VectorF bbox_max = m_wire_network->get_bbox_max();
    assert(results.size() == dim * num_vertices);
    assert(roi_size == m_transforms.size());

    if (m_formula != "") evaluate_formula(vars);

    const MatrixFr& vertices = m_wire_network->get_vertices();
    size_t seed_vertex_index = m_roi.minCoeff();
    VectorF seed_vertex = vertices.row(seed_vertex_index);
    VectorF seed_offset = VectorF::Zero(dim);
    seed_offset = (bbox_max - center).cwiseProduct(m_dof_dir) * m_value;

    for (size_t i=0; i<roi_size; i++) {
        size_t v_idx = m_roi[i];
        assert(v_idx < num_vertices);
        const MatrixF& trans = m_transforms[i];
        results.segment(v_idx*dim, dim) += trans * seed_offset;
    }
}

void VertexNormalAttribute::compute_vertex_normals_from_face(Mesh& mesh) {
    const size_t dim = mesh.get_dim();
    const size_t num_vertices = mesh.get_num_vertices();
    const size_t num_faces    = mesh.get_num_faces();
    const size_t vertex_per_face = mesh.get_vertex_per_face();

    const VectorF& normals = get_attribute(mesh, "face_normal");
    const VectorF& areas = get_attribute(mesh, "face_area");
    assert(normals.size() == 3 * num_faces);
    assert(areas.size() == num_faces);

    VectorF& v_normals = m_values;
    v_normals = VectorF::Zero(dim * num_vertices);

    for (size_t i=0; i<num_faces; i++) {
        VectorI face = mesh.get_face(i);
        assert(face.size() == vertex_per_face);
        VectorF face_normal = normals.segment(i*dim, dim);
        Float face_area = areas[i];
        for (size_t j=0; j<vertex_per_face; j++) {
            size_t vi = face[j];
            v_normals.segment(vi*dim, dim) += face_normal * face_area;
        }
    }

    for (size_t i=0; i<num_vertices; i++) {
        VectorF n = v_normals.segment(dim*i, dim);
        Float n_len = n.norm();
        if (n_len > 0.0) n /= n_len;
        v_normals.segment(dim*i, dim) = n;
    }
}

void GFXDrawUtil::drawArrow( const GFXStateBlockDesc &desc, const Point3F &start, const Point3F &end, const ColorI &color )
{   
   GFXTransformSaver saver;

   // Direction and length of the arrow.
   VectorF dir = end - start;
   F32 len = dir.len();
   dir.normalize();   
   len *= 0.2f;      

   // Base of the cone will be a distance back from the end of the arrow
   // proportional to the total distance of the arrow... 0.3f looks about right.
   Point3F coneBase = end - dir * len * 0.3f;

   // Calculate the radius of the cone given that we want the cone to have
   // an angle of 25 degrees (just because it looks good).
   F32 coneLen = ( end - coneBase ).len();
   F32 coneDiameter = mTan( mDegToRad(25.0f) ) * coneLen;

   // Draw the cone on at the arrow's tip.
   drawCone( desc, coneBase, end, coneDiameter / 2.0f, color );

   // Get the difference in length from
   // the start of the cone to the end
   // of the cylinder so we can put the
   // end of the cylinder right against where
   // the cone starts.
   Point3F coneDiff = end - coneBase;

   // Draw the cylinder.
   F32 stickRadius = len * 0.025f;   
   drawCylinder( desc, start, end - coneDiff, stickRadius, color );
}

void OBJWriter::write_mesh(Mesh& mesh) {
    using namespace OBJWriterHelper;
    std::ofstream fout(m_filename.c_str());
    if (!is_anonymous()) {
        fout << "# Generated with PyMesh" << std::endl;
    }
    VectorF texture;
    VectorI texture_indices;
    if (mesh.has_attribute("corner_texture")) {
        texture = mesh.get_attribute("corner_texture");
        const size_t num_faces = mesh.get_num_faces();
        const size_t vertex_per_face = mesh.get_vertex_per_face();
        if (texture.size() != num_faces * vertex_per_face * 2) {
            // Texture invalid.
            texture.resize(0);
        } else {
            write_texture(fout, texture);
            texture_indices.resize(num_faces * vertex_per_face);
            for (size_t i=0; i<num_faces; i++) {
                for (size_t j=0; j<vertex_per_face; j++) {
                    texture_indices[i*vertex_per_face+j] = i*vertex_per_face+j;
                }
            }
        }
    }

    write_vertices(fout, mesh.get_vertices(), mesh.get_dim());
    write_faces(fout, mesh.get_faces(), mesh.get_vertex_per_face(),
            texture_indices);
    fout.close();
}

void IsotropicDofExtractor::check_bbox_is_isotropic() const {
    VectorF half_size = 0.5 * (m_bbox_max - m_bbox_min);
    VectorF gap = (half_size.array() - m_bbox_half_size).cwiseAbs();
    if (gap.minCoeff() > 1e-12) {
        throw RuntimeError("BBox is not isotropic.");
    }
}

 void write_texture(std::ofstream& fout, const VectorF& uv) {
     assert(uv.size() % 2 == 0);
     const size_t num_uvs = uv.size() / 2;
     for (size_t i=0; i<num_uvs; i++) {
         fout << "vt " << uv[i*2] << " " << uv[i*2+1] << std::endl;
     }
 }

    VectorF convert_voxel_attribute_to_vertex_attribute(
            Mesh& mesh, const VectorF& attribute) {
        const size_t num_vertices = mesh.get_num_vertices();
        const size_t vertex_per_voxel = mesh.get_vertex_per_voxel();
        const size_t num_voxels = mesh.get_num_voxels();
        const size_t attr_size = attribute.size();
        const size_t stride = attr_size / num_voxels;

        const VectorI& voxels = mesh.get_voxels();
        if (!mesh.has_attribute("voxel_volume")) {
            mesh.add_attribute("voxel_volume");
        }
        const VectorF& weights = mesh.get_attribute("voxel_volume");

        VectorF result = VectorF::Zero(num_vertices * stride);
        VectorF result_weights = VectorF::Zero(num_vertices);
        for (size_t i=0; i<num_voxels; i++) {
            const VectorI& voxel =
                voxels.segment(i*vertex_per_voxel, vertex_per_voxel);
            Float per_vertex_weight = weights[i] / vertex_per_voxel;
            for (size_t j=0; j<vertex_per_voxel; j++) {
                result_weights[voxel[j]] += per_vertex_weight;
                result.segment(voxel[j]*stride, stride) +=
                    per_vertex_weight * attribute.segment(i*stride, stride);
            }
        }

        for (size_t i=0; i<num_vertices; i++) {
            result.segment(i*stride, stride) /= result_weights[i];
        }
        return result;
    }

    VectorF convert_face_attribute_to_vertex_attribute(
            Mesh& mesh, const VectorF& attribute) {
        const size_t num_vertices = mesh.get_num_vertices();
        const size_t vertex_per_face = mesh.get_vertex_per_face();
        const size_t num_faces = mesh.get_num_faces();
        const size_t attr_size = attribute.size();
        const size_t stride = attr_size / num_faces;

        const VectorI& faces = mesh.get_faces();
        const VectorF& weights = mesh.get_attribute("face_area");

        VectorF result = VectorF::Zero(num_vertices * stride);
        VectorF result_weights = VectorF::Zero(num_vertices);
        for (size_t i=0; i<num_faces; i++) {
            const VectorI& face =
                faces.segment(i*vertex_per_face, vertex_per_face);
            Float per_vertex_weight = weights[i] / vertex_per_face;
            for (size_t j=0; j<vertex_per_face; j++) {
                result_weights[face[j]] += per_vertex_weight;
                result.segment(face[j]*stride, stride) +=
                    per_vertex_weight * attribute.segment(i*stride, stride);
            }
        }

        for (size_t i=0; i<num_vertices; i++) {
            result.segment(i*stride, stride) /= result_weights[i];
        }
        return result;
    }

MatrixFr OffsetParameters::evaluate(const OffsetParameters::Variables& vars) {
    const size_t dim = m_wire_network->get_dim();
    const size_t size = m_wire_network->get_num_vertices();
    VectorF offset = VectorF::Ones(size * dim) * m_default_offset;
    for (auto param : m_params) {
        param->apply(offset, vars);
    }
    MatrixFr results = Eigen::Map<MatrixFr>(offset.data(), size, dim);
    return results;
}

S32 TSShapeInstance::setDetailFromPosAndScale(  const SceneRenderState *state,
                                                const Point3F &pos, 
                                                const Point3F &scale )
{
   VectorF camVector = pos - state->getDiffuseCameraPosition();
   F32 dist = getMax( camVector.len(), 0.01f );
   F32 invScale = ( 1.0f / getMax( getMax( scale.x, scale.y ), scale.z ) );

   return setDetailFromDistance( state, dist * invScale );
}

Float LinearTetrahedronIntegrator::integrate_grad_C(size_t elem_idx,
        size_t local_func_i, size_t local_func_j, const MatrixF& C) {
    const Vector4F coord(1.0/4.0, 1.0/4.0, 1.0/4.0, 1.0/4.0);
    const VectorF grad_i = m_shape_func->evaluate_grad(
            elem_idx, local_func_i, coord);
    const VectorF grad_j = m_shape_func->evaluate_grad(
            elem_idx, local_func_j, coord);
    const Float vol = m_mesh->getElementVolume(elem_idx);
    return grad_i.dot(C * grad_j) * vol;
}

void InflatorEngine::with_abs_geometry_correction(const VectorF& correction) {
    if (correction.size() != m_wire_network->get_dim()) {
        std::stringstream err_msg;
        err_msg << "Absolute geometry offset dimension mismatch.  Expect "
            << m_wire_network->get_dim() << " but get "
            << correction.size() << " instead.";
        throw RuntimeError(err_msg.str());
    }
    m_abs_correction = correction;
}

VectorF PointLocator::compute_barycentric_coord(const VectorF& v,
        size_t elem_idx) {
    const size_t dim = m_mesh->get_dim();
    MatrixF solver = m_barycentric_solvers.block(elem_idx*dim, 0, dim, dim);
    VectorF last_v = m_last_vertices.row(elem_idx);
    VectorF sol = solver * (v - last_v);
    VectorF barycentric_coord(m_vertex_per_element);
    barycentric_coord.segment(0, dim) = sol;
    barycentric_coord[dim] = 1.0 - sol.sum();
    return barycentric_coord;
}

void ClippedPolyList::generateNormals()
{
   PROFILE_SCOPE( ClippedPolyList_GenerateNormals );

   AssertFatal(mNormalList.size() == mVertexList.size(), "Normals count does not match vertex count!");    

   U32 i, polyCount;
   VectorF normal;
   PolyListIterator polyIter;
   IndexListIterator indexIter;

   Vector<VectorF>::iterator normalIter = mNormalList.begin();
   U32 n = 0;
   for ( ; normalIter != mNormalList.end(); normalIter++, n++ )
   {
       // Skip normals that already have values.
       if ( !normalIter->isZero() )
           continue;

      // Average all the face normals which 
      // share this vertex index.
      indexIter = mIndexList.begin();
      normal.zero();
      polyCount = 0;
      i = 0;

      for ( ; indexIter != mIndexList.end(); indexIter++, i++ )
      {
         if ( n != *indexIter )
            continue;

         polyIter = mPolyList.begin();
         for ( ; polyIter != mPolyList.end(); polyIter++ )
         {
            const Poly& poly = *polyIter;
            if ( i < poly.vertexStart || i > poly.vertexStart + poly.vertexCount )
               continue;

            ++polyCount;
            normal += poly.plane;
         }        
      }

      // Average it.
      if ( polyCount > 0 )
         normal /= (F32)polyCount;

      // Note: we use a temporary for the normal averaging 
      // then copy the result to limit the number of arrays
      // we're touching during the innermost loop.
      *normalIter = normal;
   }
}

 void correct_tet_orientation(const VectorF& vertices, VectorI& voxels) {
     const size_t num_voxels = voxels.size() / 4;
     for (size_t i=0; i<num_voxels; i++) {
         const VectorI tet = voxels.segment(i*4, 4);
         const Vector3F& v1 = vertices.segment(tet[0]*3, 3);
         const Vector3F& v2 = vertices.segment(tet[1]*3, 3);
         const Vector3F& v3 = vertices.segment(tet[2]*3, 3);
         const Vector3F& v4 = vertices.segment(tet[3]*3, 3);
         if (!positive_orientated(v1, v2, v3, v4)) {
             voxels[i*4]   = tet[1];
             voxels[i*4+1] = tet[0];
         }
     }
 }

void WireNetwork::translate(const VectorF& offset) {
    if (offset.size() != m_dim) {
        std::stringstream err_msg;
        err_msg << "Offset is of dim ("
            << offset.size() << "), expecting (" << m_dim << ")";
        throw RuntimeError(err_msg.str());
    }

    const size_t num_vertices = get_num_vertices();
    for (size_t i=0; i<num_vertices; i++) {
        m_vertices.row(i) += offset;
    }
    update_bbox();
}

void HoverVehicle::updateDustTrail( F32 dt )
{
    // Check to see if we're moving.

    VectorF velocityVector = getVelocity();
    F32 velocity = velocityVector.len();

    if( velocity > 2.0 )
    {
        velocityVector.normalize();
        emitDust( mDustTrailEmitter, mDataBlock->triggerTrailHeight, mDataBlock->dustTrailOffset,
                  ( U32 )( dt * 1000 * ( velocity / mDataBlock->dustTrailFreqMod ) ),
                  velocityVector );
    }
}

bool Trigger::testObject(GameBase* enter)
{
   if (mTriggerPolyhedron.pointList.size() == 0)
      return false;

   mClippedList.clear();

   SphereF sphere;
   sphere.center = (mWorldBox.minExtents + mWorldBox.maxExtents) * 0.5;
   VectorF bv = mWorldBox.maxExtents - sphere.center;
   sphere.radius = bv.len();

   enter->buildPolyList(PLC_Collision, &mClippedList, mWorldBox, sphere);
   return mClippedList.isEmpty() == false;
}