void
SoftBody::cgSolve(VectorList& x, const VectorList& b)
{
    static const double TOLERANCE = 1e-10;

    assert(x.size() == size());
    assert(b.size() == Neighborhood::MAX_SIZE * size() + 1);

    VectorList r = applyMatrixTranspose(b) - applyMatrixTranspose(applyMatrix(x));
    VectorList p(r);
    double rsold = r.dot(r);

    for (uint32_t i = 0; rsold > TOLERANCE && i < MAX_CG_ITERATIONS; ++i) {
        VectorList Ap = applyMatrixTranspose(applyMatrix(p));
        double alpha = rsold / p.dot(Ap);

        x += alpha * p;
        r -= alpha * Ap;

        double rsnew = r.dot(r);
        if (rsnew < TOLERANCE) {
            return;
        }

        p = r + (rsnew / rsold) * p;
        rsold = rsnew;
    }
}

void VectorSelector::fillVectors() {
  if (!_store) {
    return;
  }
  int current_index = _vector->currentIndex();
  VectorPtr current = selectedVector();

  QHash<QString, VectorPtr> vectors;

  VectorList vectorList = _store->getObjects<Vector>();

  VectorList::ConstIterator it = vectorList.constBegin();
  for (; it != vectorList.constEnd(); ++it) {
    VectorPtr vector = (*it);
    if (vector->isScalarList())
      continue;

    vector->readLock();
    vectors.insert(vector->CleanedName(), vector);
    vector->unlock();
  }

  QStringList list = vectors.keys();

  qSort(list);

  _vector->clear();
  foreach (const QString &string, list) {
    VectorPtr v = vectors.value(string);
    _vector->addItem(string, qVariantFromValue(v.data()));
  }

void VectorSelector::setToLastX(QString field) {
  if (!_store) {
    return;
  }
  int match = -1;
  VectorList vectors = _store->getObjects<Vector>();
  int size = vectors.size();
  for (int i = 0; i < size; ++i) {
    if (vectors.at(i)->descriptiveName() == field) {
      match = i;
    }
  }

  if (match >-1) {
    setSelectedVector(vectors.at(match));
  } else {
    CurveList objects = _store->getObjects<Curve>();
    if (objects.count()>0) {
      setSelectedVector(objects.at(objects.count()-1)->xVector());
    }
  }
}

		void
		RrtDual::getPath(VectorList& path)
		{
			Vertex i = this->end[0];
			
			while (i != this->begin[0])
			{
				path.push_front(*this->tree[0][i].q);
				i = ::boost::source(*::boost::in_edges(i, this->tree[0]).first, this->tree[0]);
			}
			
			path.push_front(*this->tree[0][i].q);
			
			i = ::boost::source(*::boost::in_edges(this->end[1], this->tree[1]).first, this->tree[1]);
			
			while (i != this->begin[1])
			{
				path.push_back(*this->tree[1][i].q);
				i = ::boost::source(*::boost::in_edges(i, this->tree[1]).first, this->tree[1]);
			}
			
			path.push_back(*this->tree[1][i].q);
		}

void testVectorList( VectorList< T > &listObject, const string &typeName )
{
   cout << "Testing a VectorList of " << typeName << " values\n";
   instructions();       // display instructions

   int choice;           // store user choice
   T value;              // store input value

   do                    // perform user-selected actions
   {
      cout << "? ";
      cin >> choice;

      switch ( choice ) 
      {
         case 1:         // insert at beginning
            cout << "Enter " << typeName << ": ";
            cin >> value;
            listObject.insertAtFront( value );
            listObject.print();
            break;
         case 2:         // insert at end
            cout << "Enter " << typeName << ": ";
            cin >> value;
            listObject.insertAtBack( value );
            listObject.print();
            break;
         case 3:         // remove from beginning
            if ( listObject.removeFromFront( value ) )
               cout << value << " removed from list\n";

            listObject.print();
            break;
         case 4:          // remove from end
            if ( listObject.removeFromBack( value ) )
               cout << value << " removed from list\n";

            listObject.print();
            break;
      }
   } while ( choice < 5 ); 

   cout << "End list test\n\n";
} 

		void
		SimpleOptimizer::process(VectorList& path)
		{
			bool changed = true;
			
			while (changed && path.size() > 2)
			{
				changed = false;
				
				VectorList::iterator i = path.begin();
				VectorList::iterator j = ++path.begin();
				VectorList::iterator k = ++++path.begin();
				
				while (i != path.end() && j != path.end() && k != path.end())
				{
					::rl::math::Real ik = this->model->distance(*i, *k);
					
					if (!this->verifier->isColliding(*i, *k, ik))
					{
						VectorList::iterator l = j;
						++j;
						++k;
						path.erase(l);
						
						if (nullptr != this->viewer)
						{
							this->viewer->drawConfigurationPath(path);
						}
						
						changed = true;
					}
					else
					{
						++i;
						++j;
						++k;
					}
				}
			}
		}

//The changes in gravitational potential (Okubo 1992)
quakelib::FloatList quakelib::SlipMap::potential_changes(const VectorList &points, const float &lambda, const float &mu, const float &cutoff) {
    quakelib::FloatList potential_changes;
    Okada block_okada;
    double potential_change, slip, US, UD, UT, L, W, c, rake_cos, rake_sin, strike_cos, strike_sin, dip, strike, xp0, yp0, xp3, yp3, x, y, xp, yp, zp;

    if (lambda <= 0 || mu <= 0) {
        throw std::invalid_argument("Lambda and mu must be greater than zero.");
    }

    for (VectorList::size_type point_id = 0; point_id != points.size(); point_id++) {
        potential_changes.push_back(0.0);
    }

    for (SlippedElementList::size_type ele_id = 0; ele_id != involved_elements.size(); ele_id++) {
        slip = involved_elements[ele_id].slip();
        c = fabs(involved_elements[ele_id].max_depth());
        rake_cos = cos(involved_elements[ele_id].rake());
        rake_sin = sin(involved_elements[ele_id].rake());

        if (fabs(rake_cos) < TRIG_TOLERANCE) {
            rake_cos = 0.0;
        }

        if (fabs(rake_sin) < TRIG_TOLERANCE) {
            rake_sin = 0.0;
        }

        US = slip * rake_cos;
        UD = slip * rake_sin;
        UT = 0.0;

        L = (involved_elements[ele_id].vert(2) - involved_elements[ele_id].vert(0)).mag();
        W = (involved_elements[ele_id].vert(1) - involved_elements[ele_id].vert(0)).mag();

        dip = involved_elements[ele_id].dip();
        strike = involved_elements[ele_id].strike();

        strike_cos = cos(strike);
        strike_sin = sin(strike);

        if (fabs(strike_cos) < TRIG_TOLERANCE) {
            strike_cos = 0.0;
        }

        if (fabs(strike_sin) < TRIG_TOLERANCE) {
            strike_sin = 0.0;
        }

        xp0 = involved_elements[ele_id].vert(1)[0];
        yp0 = involved_elements[ele_id].vert(1)[1];

        xp3 = involved_elements[ele_id].implicit_vert()[0];
        yp3 = involved_elements[ele_id].implicit_vert()[1];

        for (VectorList::size_type point_id = 0; point_id != points.size(); point_id++) {
            x = points[point_id][0];
            y = points[point_id][1];

            //if (pow(x-event_center()[0], 2) + pow(y-event_center()[1], 2) > pow( event_radius() * cutoff ,2) ) {
            // Gotta figure the cutoff for gravity changes out

            if (sqrt(pow((x-(xp0 + xp3)/2.0),2)+pow((y-(yp0+yp3)/2.0),2))/sqrt(L*W) > (cutoff + slip - 1.0) ) {
                potential_change = 0.0;
            } else {

            	xp = (x-xp0) * strike_sin + (y-yp0) * strike_cos;
				yp = -(x-xp0) * strike_cos + (y-yp0) * strike_sin;

                // CHANGE this to enable dV calculations below z=0
                zp = 0.0;

                potential_change = block_okada.calc_dV(quakelib::Vec<3>(xp,yp,zp), c, dip, L, W, US, UD, UT, lambda, mu);
            }

            //std::cout << sqrt(pow((x-(xp0 + xp3)/2.0),2)+pow((y-(yp0 + yp3)/2.0),2))/sqrt(L*W) << " " << gravity_change << std::endl;

            potential_changes[point_id] += potential_change;
        }
    }

    return potential_changes;
}

bool Rectangle::InsideShape(sf::Vector2f val, Num scale, bool shapeOnly)
{
    bool allOnRightSide = true;
    VectorList verts;
    GetVertices(&verts);
    for(int i = 0; i < 4; i++)
    {
        Num slope;
        if(verts.at((i + 1) % 4).x == verts.at(i).x)
        {
            if(verts.at((i + 1) % 4).y >= verts.at(i).y)
            {
                slope = 100000000;
            }
            else
                slope = -100000000;
        }
        else
            slope = (verts.at((i + 1) % 4).y - verts.at(i).y) / (verts.at((i + 1) % 4).x - verts.at(i).x);

        Num val1 = val.y - verts.at(i).y - slope * (val.x - verts.at(i).x);
        Num val2 = verts.at((i + 2) % 4).y - verts.at(i).y - slope * (verts.at((i + 2) % 4).x - verts.at(i).x);
        if((val1 < 0 && val2 > 0) || (val1 > 0 && val2 < 0))
            allOnRightSide = false;
    }
    return allOnRightSide;
}

void Rectangle::Init(b2World *world, b2Body *body)
{
    if(isInitted)
        return;
    ShapePart::Init(world);

    //http://www.iforce2d.net/b2dtut/fixtures
    b2PolygonShape *sd = new b2PolygonShape();
    b2FixtureDef *fd = new b2FixtureDef();
    fd->shape = sd;
    fd->friction = 0.4;
    fd->restitution = 0.3;

    fd->density = (density + 5.0) / 10.0;

    if(m_collisionGroup != -1)
        fd->filter.groupIndex = m_collisionGroup;
    if(!collide)
        fd->filter.maskBits = 0;

    VectorList verts;
    GetVertices(&verts);
    sd->Set(&verts.at(0), (int) verts.size());

    if(body)
    {
        for(unsigned i = 0; i < 4; i++)
        {
            sd->m_vertices[i].x -= body->GetPosition().x;
            sd->m_vertices[i].y -= body->GetPosition().y;
        }
        m_body = body;
        if(isStatic)
            m_body->SetType(b2_staticBody);
        //bool bodyStatic = body->GetType() == b2_staticBody;
    }
    else
    {
        for(unsigned i = 0; i < 4; i++)
        {
            sd->m_vertices[i].x -= centerX;
            sd->m_vertices[i].y -= centerY;
        }
        b2BodyDef *bd = new b2BodyDef();
        if(isStatic)
            bd->type = b2_staticBody;
        else
            bd->type = b2_dynamicBody;
        bd->position.Set(centerX, centerY);

        m_body = world->CreateBody(bd);
        if(isEditable)
        {
            bool hasJoints = false;
            for(unsigned j = 0; j < m_joints.size(); j++)
            {
                if(m_joints.at(j)->isEnabled)
                {
                    hasJoints = true;
                    break;
                }
            }
            if(!hasJoints)
                m_body->SetBullet(true);
        }
        if(isBullet)
            m_body->SetBullet(true);
    }
    //TODO:
    /*sd->userData = new Object();
    sd->userData.collide = collide;
    sd->userData.editable = (isEditable || isTank);
    sd->userData.color = color;
    sd->userData.outline = outline;
    sd->userData.terrain = terrain;
    sd->userData.undragable = undragable;
    sd->userData.isPiston = -1;
    sd->userData.isSandbox = isSandbox;*/

    m_fixture = m_body->CreateFixture(fd);

    //TODO check if mass not needed
    /*if(isStatic || bodyStatic)
        m_body->SetMass(new b2MassData());
    else
        m_body->SetMassFromShapes();*/

    for(unsigned i = 0; i < m_joints.size(); i++)
    {
        if(m_joints.at(i)->isEnabled && m_joints.at(i)->type == "FixedJoint")
        {
            ShapePart *connectedPart = m_joints.at(i)->GetOtherPart(this);
            if(connectedPart->isEnabled)
                connectedPart->Init(world, m_body);
        }
    }
}

 void CenterBody()
 {
     for(int i = 0; i < vectors.GetSize(); i++)
     vectors[i]->Vector -= vectors.Find("CenterBody")->Vector;
 }

    Bones()
    {
        vectors.AddNode(IDVector("CenterRHand", Color(50, 0, 0)));
        vectors.AddNode(IDVector("CenterRForearm", Color(100, 0, 0)));
        vectors.AddNode(IDVector("CenterRUpperarm", Color(150, 0, 0)));

        vectors.AddNode(IDVector("PointRFoot", Color(0, 250, 0)));
        vectors.AddNode(IDVector("CenterRFoot", Color(0, 200, 0)));
        vectors.AddNode(IDVector("CenterRKnee", Color(0, 150, 0)));
        vectors.AddNode(IDVector("CenterRHip", Color(0, 100, 0)));

        vectors.AddNode(IDVector("CenterLHand", Color(0, 0, 100)));
        vectors.AddNode(IDVector("CenterLForearm", Color(0, 0, 150)));
        vectors.AddNode(IDVector("CenterLUpperarm", Color(0, 0, 200)));

        vectors.AddNode(IDVector("PointLFoot", Color(150, 50, 0)));
        vectors.AddNode(IDVector("CenterLFoot", Color(100, 50, 0)));
        vectors.AddNode(IDVector("CenterLKnee", Color(50, 50, 0)));
        vectors.AddNode(IDVector("CenterLHip", Color(0, 0, 250)));

        vectors.AddNode(IDVector("PointHead", Color(250, 0, 0)));
        vectors.AddNode(IDVector("CenterHead", Color(200, 0, 0)));
        vectors.AddNode(IDVector("CenterBody", Color(0, 50, 0)));

        rArmGravity = false;
        lArmGravity = false;
        rLegGravity = false;
        lLegGravity = false;

        Time = 1.f;
    }