TEFUNC void keyHandler(unsigned char key) {
	switch (key) {
		case '1': constraintSystem.setComputationAlgorithm(1); break;
		case '2': constraintSystem.setComputationAlgorithm(2); break;
		case '3': constraintSystem.setComputationAlgorithm(3); break;
		case '4': constraintSystem.setComputationAlgorithm(4); break;
		case 'a': 
			if (loop) {
				loop = false;
			} else {
				loop = true;
			}
		break;
		case 'm': 
			if (move) {
				move = false;
			} else {
				move = true;
			}
		break;
		case 'p': postStabilization = true; break;
		case 'P': postStabilization = false; break;
	}

}
/**
 * \brief Start-Funktion der Testumgebung
 *
 * Diese Funktion wird direkt von display() aufgerufen. Hier
 * sollte alles reingeschrieben werden, was für die einzelnen
 * Tests nötig ist.
 */
TEFUNC void displayLoop() {
	if (loop) {
		//! gravitation aufrechnen
		bodySystem.addGravity();
	
		geometrySystem.resolveCollisions(10);
	
		constraintSystem.step();
		if (postStabilization)
			constraintSystem.computePostStabilization ();
		
		//! Integriere so weit, wie der letzte simulations Schritt brauchte
		//bodySystem.integrateRungeKutta(getLastTime());
		//bodySystem.integrateEuler(getLastTime());
		
		bodySystem.integrateEulerVelocities(10);
		geometrySystem.resolveContacts(10);
		bodySystem.integrateEulerPositions(10);
					
		// für schweben : -1.170
		sph1acc->addForce(-1.10 * SimonState::exemplar()->getGravityVector()* sph1acc->getMass());
		
		if (move) {
			sph6acc->addForce(Vec3(5.0,5.0,0.0));
			sph7acc->addForce(Vec3(-5.0,5.0,0.0));
			sph8acc->addForce(Vec3(0.0,5.0,5.0));
			sph9acc->addForce(Vec3(0.0,5.0,-5.0));
		}
		//sph->addForce(50 * Vec3(1.0,0.0,0.0));
	
		
	}
	geometrySystem.drawGeometries();
	//cout << getLastTime() << endl;
}
TEFUNC void keyHandler(unsigned char key) {
	switch (key) {
		case '1': cs.setComputationAlgorithm(1); break;
		case '2': cs.setComputationAlgorithm(2); break;
		case '3': cs.setComputationAlgorithm(3); break;
		case '4': cs.setComputationAlgorithm(4); break;
		case 'a': integrate = true; doConstraints = true; break;
		case 'A': integrate = false; doConstraints = false; break;
		case 't': if (doTorque) {doTorque=false;} else {doTorque=true;} break;
		case 'b': 
			if (PrimaryConstraint::doesBaumgarteStabilisation()) {
				PrimaryConstraint::doBaumgarteStabilisation(false);
			} else {
				PrimaryConstraint::doBaumgarteStabilisation(true);
			}
			break;
		default:break;
		case 'p': postStabilization = true; break;
		case 'P': postStabilization = false; break;
	}
}
Ejemplo n.º 4
0
Fix Fix::getRelabelTuple(ConstraintSystem &cs, FixKind kind,
                         ArrayRef<Identifier> names) {
  assert(isRelabelTupleKind(kind) && "Not a tuple-relabel fix");
  Fix result(kind, cs.RelabelTupleNames.size());
  auto &allocator = cs.getAllocator();

  // Copy the names and indices.
  Identifier *namesCopy = allocator.Allocate<Identifier>(names.size());
  memcpy(namesCopy, names.data(), names.size() * sizeof(Identifier));
  cs.RelabelTupleNames.push_back({namesCopy, names.size()});

  return result;
}
Ejemplo n.º 5
0
      // Low level metod impose a ConstraintSystem object to the solver
   int GeneralConstraint::constraintToSolver( ConstraintSystem& system,
                                              gnssDataMap& gdsMap )
   {
      try
      {
         Vector<double> meas;
         Matrix<double> design;
         Matrix<double> covariance;

         system.constraintMatrix(getVariables(),meas,design,covariance);
         
         /*
         cout << StringUtils::asString(getVariables()) << endl;

         cout << meas << endl;

         cout << design << endl;

         cout << covariance << endl;
         */

         if(meas.size()>0)
         {    
            solver.kFilter.MeasUpdate(meas,design,covariance);

            Vector<double> measVector = solver.getEquationSystem()
                                              .getPrefitsVector();

            Matrix<double> designMatrix = solver.getEquationSystem()
                                                .getGeometryMatrix();

            solver.solution = solver.kFilter.xhat;
            solver.covMatrix = solver.kFilter.P;
            solver.postfitResiduals = measVector 
                                     -(designMatrix * solver.solution);

            solver.postCompute(gdsMap);
         }

         return 0;
      }
      catch (...)
      {
         return -1;
      }

   }  // End of method 'GeneralConstraint::constraint('
Ejemplo n.º 6
0
// Suggest a force-unwrap.
static void offerForceUnwrapFixit(ConstraintSystem &CS, Expr *expr) {
  auto diag = CS.TC.diagnose(expr->getLoc(), diag::unwrap_with_force_value);

  // If expr is optional as the result of an optional chain and this last
  // dot isn't a member returning optional, then offer to force the last
  // link in the chain, rather than an ugly parenthesized postfix force.
  if (auto optionalChain = dyn_cast<OptionalEvaluationExpr>(expr)) {
    if (auto dotExpr =
        dyn_cast<UnresolvedDotExpr>(optionalChain->getSubExpr())) {
      auto bind = dyn_cast<BindOptionalExpr>(dotExpr->getBase());
      if (bind && !CS.getType(dotExpr)->getOptionalObjectType()) {
        diag.fixItReplace(SourceRange(bind->getLoc()), "!");
        return;
      }
    }
  }

  if (expr->canAppendPostfixExpression(true)) {
    diag.fixItInsertAfter(expr->getEndLoc(), "!");
  } else {
    diag.fixItInsert(expr->getStartLoc(), "(")
    .fixItInsertAfter(expr->getEndLoc(), ")!");
  }
}
Ejemplo n.º 7
0
int main(void)
{
    /**
     * Unit tests for environment
     */
    try
    {
      	SimpleSP spa;
        Coordonate<2,int> coord;
        
      	// Test for addObject()
      	
        // Add some resources
      	Environment<2, int, int> env;

        std::vector<AResource*> res;
        for(unsigned i = 0; i < 5; i++)
        {
            coord[0] = i;
            coord[1] = 1;
            res.push_back(new Resource<2,int,int>(coord, 100, false, spa));
        }
        
        env.addObject(res);
        
        if(env.getResources() != res)
			throw std::runtime_error("Error in expectations for first test");

        // Add some taskSpots
        Task t;
        StepConstraint sc(0, t, ConstraintComp::GREATER, 500);
        ConstraintSystem constraintSystem;
        constraintSystem.push(&sc);
        
        std::vector<ATaskSpot*> ts;
        for(unsigned i = 0; i < 5; i++)
        {
            coord[0] = i;
            coord[1] = 4;
            ts.push_back(new TaskSpot<2,int>(coord, std::ref(t), [](int& i, double _time){ return i+0.001*_time; }));
        }
        
        env.addObject(ts);
        
        if(env.getTaskSpots() != ts)
			throw std::runtime_error("Error in expectations for second test");

		// Test for update()
		env.update(0);
		env.update(-10);
		env.update(10);
			
      	// Test for _dump()
      	env.dump();
    }
    catch(exception& e)
    {
        logger(Logger::ERROR) << e.what();
        logger << "FATAL ERROR - EXIT NOW !";
    }
    
    return 0;
    
}
/**
 * \brief Start-Funktion der Testumgebung
 *
 * Diese Funktion wird einmal beim Starten der Testumgebung
 * aufgerufen. Hier sollte alles reingeschrieben werden,
 * was für die initialisierung der einzelnen Tests nötig ist.
 */
TEFUNC void initialize(int /*argc*/, char** /*argv*/) {

	// ------- Ein Paar Ausgaben, um die Funktionsweise vom Logging zu zeigen. -----//
	SimonState::exemplar()->errors <<"Simon: Dies ist eine Fehler-Meldung!!!!" << SimonState::endm;
	SimonState::exemplar()->errors <<"Simon: Dies ist eine weitere Fehler-Meldung!!!!" << SimonState::endm;
	SimonState::exemplar()->errors <<"Simon: Dies ist eine weitere Fehler-Meldung2!!!!" << SimonState::endm;
	SimonState::exemplar()->errors <<"Simon: Dies ist eine weitere Fehler-Meldung3!!!!" << SimonState::endm;
	SimonState::exemplar()->errors <<"Simon: Dies ist eine weitere Fehler-Meldung4!!!!" << SimonState::endm;

	SimonState::exemplar()->messages <<"Simon: Dies ist eine normale Message!!!!" << SimonState::endm;
	SimonState::exemplar()->messages <<"Simon: Dies ist eine witer ueberfluessige Meldung!!!!" << SimonState::endm;
	SimonState::exemplar()->messages <<"Collision: Hier spricht das Kollisionssystem!!!" << SimonState::endm;

	// ---------------- Ende der Log-Tests -----------------//


	// use low gravity for testing
 	//Vector3<float> gravity(0.0, -0.00089, 0.0);
 	//SimonState::exemplar()->setGravityVector(gravity);

	Id planeId(Id::typePlane, 0);

 	// erzeuge einen Festkörper für die Ebene
	SmartPointer<RigidBody> body = bodySystem.create(planeId);
	
	// die Ebene sollte nicht vom Integrator behandelt werden.
	body->setIsDynamicFlag(false);

	//  etwas drehen, damit die Ebene richtig ausgerichtet ist
	body->setOrientation(Quaternion(0, Vector3<float>(1,0,0)));

	body->setPosition(Vector3<float>(0.0,-200.0,0.0));

	// erzeuge eine Ebene, mit dem oben defineriten Festkörper
	SmartPointer<Geometry> plane = geometrySystem.createPlane(body);


	

	Id sphereId1(Id::typeSphere, 0);
	Id sphereId2(Id::typeSphere, 1);
	Id sphereId3(Id::typeSphere, 2);
	Id sphereId4(Id::typeSphere, 3);
	Id sphereId5(Id::typeSphere, 4);
	
	Id sphereId6(Id::typeSphere, 5);
	Id sphereId7(Id::typeSphere, 6);
	
	Id sphereId8(Id::typeSphere, 7);
	Id sphereId9(Id::typeSphere, 8);
	

	
	RigidBodyPtr sphb1 = bodySystem.create(sphereId1);
	RigidBodyPtr sphb2 = bodySystem.create(sphereId2);
	RigidBodyPtr sphb3 = bodySystem.create(sphereId3);
	RigidBodyPtr sphb4 = bodySystem.create(sphereId4);
	RigidBodyPtr sphb5 = bodySystem.create(sphereId5);
	
	
	//Kontrollkugeln
	RigidBodyPtr sphb6 = bodySystem.create(sphereId6);
	RigidBodyPtr sphb7 = bodySystem.create(sphereId7);
	
	//Fallende Kugeln
	RigidBodyPtr sphb8 = bodySystem.create(sphereId8);
	RigidBodyPtr sphb9 = bodySystem.create(sphereId9);
	
	
	sphb1->setMass(10000.0);
	sphb2->setMass(100.0);
	sphb3->setMass(100.0);
	sphb4->setMass(100.0);
	sphb5->setMass(100.0);
	
	sphb6->setMass(100.0);
	sphb7->setMass(100.0);
	sphb8->setMass(100.0);
	sphb9->setMass(100.0);
	
	
	
	
	SmartPointer<Geometry> sph1 = geometrySystem.createSphere(sphb1, 50);
	SmartPointer<Geometry> sph2 = geometrySystem.createSphere(sphb2, 30);
	SmartPointer<Geometry> sph3 = geometrySystem.createSphere(sphb3, 30);
	SmartPointer<Geometry> sph4 = geometrySystem.createSphere(sphb4, 30);
	SmartPointer<Geometry> sph5 = geometrySystem.createSphere(sphb5, 30);
	
	SmartPointer<Geometry> sph6 = geometrySystem.createSphere(sphb6, 10);
	SmartPointer<Geometry> sph7 = geometrySystem.createSphere(sphb7, 10);
	SmartPointer<Geometry> sph8 = geometrySystem.createSphere(sphb8, 10);
	SmartPointer<Geometry> sph9 = geometrySystem.createSphere(sphb9, 10);
	
	
	
	sph1->setBounciness(0.0);
	sph2->setBounciness(0.0);
	sph3->setBounciness(0.0);
	sph4->setBounciness(0.0);
	sph5->setBounciness(0.0);
	sph6->setBounciness(0.0);
	sph7->setBounciness(0.0);
	sph8->setBounciness(0.0);
	sph9->setBounciness(0.0);

	
	Id capsuleId1(Id::typeCapsule, 0);
	Id capsuleId2(Id::typeCapsule, 1);
	Id capsuleId3(Id::typeCapsule, 2);
	Id capsuleId4(Id::typeCapsule, 3);
	Id capsuleId5(Id::typeCapsule, 4);
	
	Id capsuleId6(Id::typeCapsule, 5);
	Id capsuleId7(Id::typeCapsule, 6);
	Id capsuleId8(Id::typeCapsule, 7);
	Id capsuleId9(Id::typeCapsule, 8);

	// hohle neuen RigidBody 
    	RigidBodyPtr capsb1 = bodySystem.create(capsuleId1);
	RigidBodyPtr capsb2 = bodySystem.create(capsuleId2);
	RigidBodyPtr capsb3 = bodySystem.create(capsuleId3);
	RigidBodyPtr capsb4 = bodySystem.create(capsuleId4);
	RigidBodyPtr capsb5 = bodySystem.create(capsuleId5);
	RigidBodyPtr capsb6 = bodySystem.create(capsuleId6);
	RigidBodyPtr capsb7 = bodySystem.create(capsuleId7);
	RigidBodyPtr capsb8 = bodySystem.create(capsuleId8);
	RigidBodyPtr capsb9 = bodySystem.create(capsuleId9);
	
	// setzen einiger attribute
	
	sphb1->setPosition(Vector3<float>(0.0,700.0,0.0));//Kopf
	capsb1->setPosition(Vector3<float>(0.0,520.0,0.0));//Körper
	
	capsb2->setPosition(Vector3<float>(120.0,600.0,0.0));//linker Oberarm
	capsb3->setPosition(Vector3<float>(120.0,460.0,0.0));//linker Unterarm
	sphb2->setPosition(Vector3<float>(120.0,380.0,0.0));//linke Hand
	sphb6->setPosition(Vector3<float>(120.0,340.0,0.0));//linke Hand Steuerung
	
	capsb4->setPosition(Vector3<float>(-120.0,600.0,0.0));//rechter Oberarm
	capsb5->setPosition(Vector3<float>(-120.0,460.0,0.0));//rechter Unterarm
	sphb3->setPosition(Vector3<float>(-120.0,380.0,0.0));//rechte Hand
	sphb7->setPosition(Vector3<float>(-120.0,340.0,0.0));//rechte Hand Steuerung
	
	capsb6->setPosition(Vector3<float>(40.0,300.0,0.0));//linker Oberschenkel
	capsb7->setPosition(Vector3<float>(40.0,160.0,0.0));//linker Unterschenkel
	sphb4->setPosition(Vector3<float>(40.0,80.0,0.0));//linker Fuss
	sphb8->setPosition(Vector3<float>(40.0,40.0,0.0));//linker Fuss Steuerung
	
	
	capsb8->setPosition(Vector3<float>(-40.0,300.0,0.0));//rechter Oberschenkel
	capsb9->setPosition(Vector3<float>(-40.0,160.0,0.0));//rechter Unterschenkel
	sphb5->setPosition(Vector3<float>(-40.0,80.0,0.0));//rechter Fuss
	sphb9->setPosition(Vector3<float>(-40.0,40.0,0.0));//rechter Fuss Steuerung
	
	capsb1->setOrientation(Quaternion(0,Vec3(0.0,0.0,1.0)));
	//capsb2->setOrientation(Quaternion(M_PI/2,Vec3(0.0,0.0,1.0)));
	//capsb3->setOrientation(Quaternion(M_PI/2,Vec3(0.0,0.0,1.0)));
	//capsb4->setOrientation(Quaternion(M_PI/2,Vec3(0.0,0.0,1.0)));
	//capsb5->setOrientation(Quaternion(M_PI/2,Vec3(0.0,0.0,1.0)));
	//capsb6->setOrientation(Quaternion(M_PI/2,Vec3(0.0,0.0,1.0)));
	//capsb7->setOrientation(Quaternion(M_PI/2,Vec3(0.0,0.0,1.0)));
	//capsb8->setOrientation(Quaternion(M_PI/2,Vec3(0.0,0.0,1.0)));
	//capsb9->setOrientation(Quaternion(M_PI/2,Vec3(0.0,0.0,1.0)));
	
	capsb1->setMass(100.0);
	capsb2->setMass(100.0);
	capsb3->setMass(100.0);
	capsb4->setMass(100.0);
	capsb5->setMass(100.0);
	capsb6->setMass(100.0);
	capsb7->setMass(100.0);
	capsb8->setMass(100.0);
	capsb9->setMass(100.0);
	
	// setzen der geometrie
	GeometryPtr caps1 = geometrySystem.createCapsule(capsb1, 40, 160);
	GeometryPtr caps2 = geometrySystem.createCapsule(capsb2, 30, 60);
	GeometryPtr caps3 = geometrySystem.createCapsule(capsb3, 30, 60);
	GeometryPtr caps4 = geometrySystem.createCapsule(capsb4, 30, 60);
	GeometryPtr caps5 = geometrySystem.createCapsule(capsb5, 30, 60);
	GeometryPtr caps6 = geometrySystem.createCapsule(capsb6, 30, 60);
	GeometryPtr caps7 = geometrySystem.createCapsule(capsb7, 30, 60);
	GeometryPtr caps8 = geometrySystem.createCapsule(capsb8, 30, 60);
	GeometryPtr caps9 = geometrySystem.createCapsule(capsb9, 30, 60);
	
	sph1->setBounciness(0.3);


	// ---------------- Ende: Große Kapsel in der Mitte ----------------//

	// ---------------- Constraint zwischen Kugel und Kapsel ---- //

	Id jointId1(Id::typeBallJoint, 0);
	Id jointId2(Id::typeBallJoint, 1);
	Id jointId3(Id::typeBallJoint, 2);
	Id jointId4(Id::typeBallJoint, 3);
	Id jointId5(Id::typeBallJoint, 4);
	Id jointId6(Id::typeBallJoint, 5);
	Id jointId7(Id::typeBallJoint, 6);
	Id jointId8(Id::typeBallJoint, 7);
	Id jointId9(Id::typeBallJoint, 8);
	Id jointId10(Id::typeBallJoint, 9);
	Id jointId11(Id::typeBallJoint, 10);
	Id jointId12(Id::typeBallJoint, 11);
	Id jointId13(Id::typeBallJoint, 12);
	Id jointId14(Id::typeBallJoint, 13);
	Id jointId15(Id::typeBallJoint, 14);
	Id jointId16(Id::typeBallJoint, 15);
	Id jointId17(Id::typeBallJoint, 16);

	constraintSystem.createBallAndSocketConstraint(jointId1, sphb1, capsb1, Vec3(0,-70,0), Vec3(0,110,0));
	
	//constraintSystem.createBallAndSocketConstraint(jointId2, capsb2, capsb3, Vec3(0,-65,0), Vec3(0,65,0));
	constraintSystem.createHingeConstraint(jointId2, capsb2, capsb3, Vec3(0,-65,0), Vec3(0,65,0), Vec3(1,0,0), Vec3(1,0,0));
	constraintSystem.createBallAndSocketConstraint(jointId3, capsb1, capsb2, Vec3(100,80,0), Vec3(0,30,0));
	constraintSystem.createBallAndSocketConstraint(jointId10, capsb3, sphb2, Vec3(0,-70,0), Vec3(0,40,0));
	
	constraintSystem.createBallAndSocketConstraint(jointId4, capsb1, capsb4, Vec3(-100,80,0), Vec3(0,30,0));
	//constraintSystem.createBallAndSocketConstraint(jointId5, capsb4, capsb5, Vec3(0,-65,0), Vec3(0,65,0));
	constraintSystem.createHingeConstraint(jointId5, capsb4, capsb5, Vec3(0,-65,0), Vec3(0,65,0), Vec3(1,0,0), Vec3(1,0,0));
	constraintSystem.createBallAndSocketConstraint(jointId11, capsb5, sphb3, Vec3(0,-70,0), Vec3(0,40,0));
	
	//constraintSystem.createBallAndSocketConstraint(jointId6, capsb6, capsb7, Vec3(0,-65,0), Vec3(0,65,0));
	constraintSystem.createHingeConstraint(jointId6, capsb6, capsb7, Vec3(0,-65,0), Vec3(0,65,0), Vec3(1,0,0), Vec3(1,0,0));
	constraintSystem.createBallAndSocketConstraint(jointId7, capsb1, capsb6, Vec3(50,-160,0), Vec3(0,30,0));
	constraintSystem.createBallAndSocketConstraint(jointId12, capsb7, sphb4, Vec3(0,-70,0), Vec3(0,40,0));
	
	
	//constraintSystem.createBallAndSocketConstraint(jointId9, capsb8, capsb9, Vec3(0,-65,0), Vec3(0,65,0));
	constraintSystem.createHingeConstraint(jointId9, capsb8, capsb9, Vec3(0,-65,0), Vec3(0,65,0), Vec3(1,0,0), Vec3(1,0,0));
	constraintSystem.createBallAndSocketConstraint(jointId8, capsb1, capsb8, Vec3(-50,-160,0), Vec3(0,30,0));
	constraintSystem.createBallAndSocketConstraint(jointId13, capsb9, sphb5, Vec3(0,-70,0), Vec3(0,40,0));
	
	//Steuerungen
	constraintSystem.createBallAndSocketConstraint(jointId14, sphb2, sphb6, Vec3(0,-50,0), Vec3(0,0,0));
	constraintSystem.createBallAndSocketConstraint(jointId15, sphb3, sphb7, Vec3(0,-50,0), Vec3(0,0,0));
	constraintSystem.createBallAndSocketConstraint(jointId16, sphb4, sphb8, Vec3(0,-50,0), Vec3(0,0,0));
	constraintSystem.createBallAndSocketConstraint(jointId17, sphb5, sphb9, Vec3(0,-50,0), Vec3(0,0,0));

	// ---------------- Ende: Constraint zwischen Kugel und Kapsel ---- //

	constraintSystem.buildGraphs();
	
	constraintSystem.setTau(60);

	SimonState::exemplar()->setViscositySlowdownAngular(0.8);
 	SimonState::exemplar()->setViscositySlowdownLinear(0.98);

	sph1acc = sphb1;
	sph6acc = sphb6;
	sph7acc = sphb7;
	sph8acc = sphb8;
	sph9acc = sphb9;

}
Ejemplo n.º 9
0
SolutionCompareResult
ConstraintSystem::compareSolutions(ConstraintSystem &cs,
                                   ArrayRef<Solution> solutions,
                                   const SolutionDiff &diff,
                                   unsigned idx1, unsigned idx2) {

  if (cs.TC.getLangOpts().DebugConstraintSolver) {
    auto &log = cs.getASTContext().TypeCheckerDebug->getStream();
    log.indent(cs.solverState->depth * 2)
      << "comparing solutions " << idx1 << " and " << idx2 <<"\n";
  }

  // Whether the solutions are identical.
  bool identical = true;

  // Compare the fixed scores by themselves.
  if (solutions[idx1].getFixedScore() != solutions[idx2].getFixedScore()) {
    return solutions[idx1].getFixedScore() < solutions[idx2].getFixedScore()
             ? SolutionCompareResult::Better
             : SolutionCompareResult::Worse;
  }
  
  // Compute relative score.
  unsigned score1 = 0;
  unsigned score2 = 0;
  
  auto foundRefinement1 = false;
  auto foundRefinement2 = false;

  bool isStdlibOptionalMPlusOperator1 = false;
  bool isStdlibOptionalMPlusOperator2 = false;

  // Compare overload sets.
  for (auto &overload : diff.overloads) {
    auto choice1 = overload.choices[idx1];
    auto choice2 = overload.choices[idx2];

    // If the systems made the same choice, there's nothing interesting here.
    if (sameOverloadChoice(choice1, choice2))
      continue;

    auto decl1 = choice1.getDecl();
    auto dc1 = decl1->getDeclContext();
    auto decl2 = choice2.getDecl();
    auto dc2 = decl2->getDeclContext();

    // The two systems are not identical. If the decls in question are distinct
    // protocol members, let the checks below determine if the two choices are
    // 'identical' or not. This allows us to structurally unify disparate
    // protocol members during overload resolution.
    // FIXME: Along with the FIXME below, this is a hack to work around
    // problems with restating requirements in protocols.
    bool decl1InSubprotocol = false;
    bool decl2InSubprotocol = false;
    if ((dc1->getContextKind() == DeclContextKind::NominalTypeDecl) &&
        (dc1->getContextKind() == dc2->getContextKind())) {
      
      auto ntd1 = dyn_cast<NominalTypeDecl>(dc1);
      auto ntd2 = dyn_cast<NominalTypeDecl>(dc2);
      
      identical = (ntd1 != ntd2) &&
                  (ntd1->getKind() == DeclKind::Protocol) &&
                  (ntd2->getKind() == DeclKind::Protocol);

      // FIXME: This hack tells us to prefer members of subprotocols over
      // those of the protocols they inherit, if all else fails.
      // If we were properly handling overrides of protocol members when
      // requirements get restated, it would not be necessary.
      if (identical) {
        decl1InSubprotocol = cast<ProtocolDecl>(ntd1)->inheritsFrom(
                               cast<ProtocolDecl>(ntd2));
        decl2InSubprotocol = cast<ProtocolDecl>(ntd2)->inheritsFrom(
                               cast<ProtocolDecl>(ntd1));
      }
    } else {
      identical = false;
    }
    
    // If the kinds of overload choice don't match...
    if (choice1.getKind() != choice2.getKind()) {
      identical = false;
      
      // A declaration found directly beats any declaration found via dynamic
      // lookup, bridging, or optional unwrapping.
      if (choice1.getKind() == OverloadChoiceKind::Decl &&
          (choice2.getKind() == OverloadChoiceKind::DeclViaDynamic || 
           choice2.getKind() == OverloadChoiceKind::DeclViaBridge ||
           choice2.getKind() == OverloadChoiceKind::DeclViaUnwrappedOptional)) {
        ++score1;
        continue;
      }

      if ((choice1.getKind() == OverloadChoiceKind::DeclViaDynamic ||
           choice1.getKind() == OverloadChoiceKind::DeclViaBridge ||
           choice1.getKind() == OverloadChoiceKind::DeclViaUnwrappedOptional) &&
          choice2.getKind() == OverloadChoiceKind::Decl) {
        ++score2;
        continue;
      }

      continue;
    }

    // The kinds of overload choice match, but the contents don't.
    auto &tc = cs.getTypeChecker();
    switch (choice1.getKind()) {
    case OverloadChoiceKind::TupleIndex:
    case OverloadChoiceKind::TypeDecl:
      continue;

    case OverloadChoiceKind::BaseType:
      llvm_unreachable("Never considered different");

    case OverloadChoiceKind::DeclViaDynamic:
    case OverloadChoiceKind::Decl:
    case OverloadChoiceKind::DeclViaBridge:
    case OverloadChoiceKind::DeclViaUnwrappedOptional:
      break;
    }
    
    // Determine whether one declaration is more specialized than the other.
    bool firstAsSpecializedAs = false;
    bool secondAsSpecializedAs = false;
    if (isDeclAsSpecializedAs(tc, cs.DC, decl1, decl2)) {
      ++score1;
      firstAsSpecializedAs = true;
    }
    if (isDeclAsSpecializedAs(tc, cs.DC, decl2, decl1)) {
      ++score2;
      secondAsSpecializedAs = true;
    }

    // If each is as specialized as the other, and both are constructors,
    // check the constructor kind.
    if (firstAsSpecializedAs && secondAsSpecializedAs) {
      if (auto ctor1 = dyn_cast<ConstructorDecl>(decl1)) {
        if (auto ctor2 = dyn_cast<ConstructorDecl>(decl2)) {
          if (ctor1->getInitKind() != ctor2->getInitKind()) {
            if (ctor1->getInitKind() < ctor2->getInitKind())
              ++score1;
            else
              ++score2;
          } else if (ctor1->getInitKind() ==
                     CtorInitializerKind::Convenience) {
            
            // If both are convenience initializers, and the instance type of
            // one is a subtype of the other's, favor the subtype constructor.
            auto resType1 = ctor1->getResultType();
            auto resType2 = ctor2->getResultType();
            
            if (!resType1->isEqual(resType2)) {
              if (tc.isSubtypeOf(resType1, resType2, cs.DC)) {
                ++score1;
              } else if (tc.isSubtypeOf(resType2, resType1, cs.DC)) {
                ++score2;
              }
            }
          }
        }
      }
    }

    // If both declarations come from Clang, and one is a type and the other
    // is a function, prefer the function.
    if (decl1->hasClangNode() &&
        decl2->hasClangNode() &&
        ((isa<TypeDecl>(decl1) &&
          isa<AbstractFunctionDecl>(decl2)) ||
         (isa<AbstractFunctionDecl>(decl1) &&
          isa<TypeDecl>(decl2)))) {
      if (isa<TypeDecl>(decl1))
        ++score2;
      else
        ++score1;
    }

    // A class member is always better than a curried instance member.
    // If the members agree on instance-ness, a property is better than a
    // method (because a method is usually immediately invoked).
    if (!decl1->isInstanceMember() && decl2->isInstanceMember())
      ++score1;
    else if (!decl2->isInstanceMember() && decl1->isInstanceMember())
      ++score2;
    else if (isa<VarDecl>(decl1) && isa<FuncDecl>(decl2))
      ++score1;
    else if (isa<VarDecl>(decl2) && isa<FuncDecl>(decl1))
      ++score2;
    
    // If we haven't found a refinement, record whether one overload is in
    // any way more constrained than another. We'll only utilize this
    // information in the case of a potential ambiguity.
    if (!(foundRefinement1 && foundRefinement2)) {
      if (isDeclMoreConstrainedThan(decl1, decl2)) {
        foundRefinement1 = true;
      }
      
      if (isDeclMoreConstrainedThan(decl2, decl1)) {
        foundRefinement2 = true;
      }
    }
     
    // If we still haven't found a refinement, check if there's a parameter-
    // wise comparison between an empty existential collection and a non-
    // existential type.
    if (!(foundRefinement1 && foundRefinement2)) {
      if (hasEmptyExistentialParameterMismatch(decl1, decl2)) {
        foundRefinement1 = true;
      }
      
      if (hasEmptyExistentialParameterMismatch(decl2, decl1)) {
        foundRefinement2 = true;
      }
    }

    // FIXME: The rest of the hack for restating requirements.
    if (!(foundRefinement1 && foundRefinement2)) {
      if (identical && decl1InSubprotocol != decl2InSubprotocol) {
        foundRefinement1 = decl1InSubprotocol;
        foundRefinement2 = decl2InSubprotocol;
      }
    }

    // FIXME: Lousy hack for ?? to prefer the catamorphism (flattening)
    // over the mplus (non-flattening) overload if all else is equal.
    if (decl1->getName().str() == "??") {
      assert(decl2->getName().str() == "??");

      auto check = [](const ValueDecl *VD) -> bool {
        if (!VD->getModuleContext()->isStdlibModule())
          return false;
        auto fnTy = VD->getType()->castTo<AnyFunctionType>();
        if (!fnTy->getResult()->getAnyOptionalObjectType())
          return false;

        // Check that the standard library hasn't added another overload of
        // the ?? operator.
        auto inputTupleTy = fnTy->getInput()->castTo<TupleType>();
        auto inputTypes = inputTupleTy->getElementTypes();
        assert(inputTypes.size() == 2);
        assert(inputTypes[0]->getAnyOptionalObjectType());
        auto autoclosure = inputTypes[1]->castTo<AnyFunctionType>();
        assert(autoclosure->isAutoClosure());
        auto secondParamTy = autoclosure->getResult();
        assert(secondParamTy->getAnyOptionalObjectType());
        (void)secondParamTy;

        return true;
      };

      isStdlibOptionalMPlusOperator1 = check(decl1);
      isStdlibOptionalMPlusOperator2 = check(decl2);
    }
  }

  // Compare the type variable bindings.
  auto &tc = cs.getTypeChecker();
  for (auto &binding : diff.typeBindings) {
    // If the type variable isn't one for which we should be looking at the
    // bindings, don't.
    if (!binding.typeVar->getImpl().prefersSubtypeBinding())
      continue;

    auto type1 = binding.bindings[idx1];
    auto type2 = binding.bindings[idx2];

    // Strip any initializers from tuples in the type; they aren't
    // to be compared.
    type1 = stripInitializers(type1);
    type2 = stripInitializers(type2);

    // If the types are equivalent, there's nothing more to do.
    if (type1->isEqual(type2))
      continue;
    
    // If either of the types still contains type variables, we can't
    // compare them.
    // FIXME: This is really unfortunate. More type variable sharing
    // (when it's sane) would help us do much better here.
    if (type1->hasTypeVariable() || type2->hasTypeVariable()) {
      identical = false;
      continue;
    }

    // If one type is an implicitly unwrapped optional of the other,
    // prefer the non-optional.    
    bool type1Better = false;
    bool type2Better = false;
    if (auto type1Obj = type1->getImplicitlyUnwrappedOptionalObjectType()) {
      if (type1Obj->isEqual(type2))
        type2Better = true;
    }
    if (auto type2Obj = type2->getImplicitlyUnwrappedOptionalObjectType()) {
      if (type2Obj->isEqual(type1))
        type1Better = true;
    }

    if (type1Better || type2Better) {
      if (type1Better)
        ++score1;
      if (type2Better)
        ++score2;
      continue;
    }

    // If one type is a subtype of the other, but not vice-versa,
    // we prefer the system with the more-constrained type.
    // FIXME: Collapse this check into the second check.
    type1Better = tc.isSubtypeOf(type1, type2, cs.DC);
    type2Better = tc.isSubtypeOf(type2, type1, cs.DC);
    if (type1Better || type2Better) {
      if (type1Better)
        ++score1;
      if (type2Better)
        ++score2;

      // Prefer the unlabeled form of a type.
      auto unlabeled1 = type1->getUnlabeledType(cs.getASTContext());
      auto unlabeled2 = type2->getUnlabeledType(cs.getASTContext());
      if (unlabeled1->isEqual(unlabeled2)) {
        if (type1->isEqual(unlabeled1)) {
          ++score1;
          continue;
        }
        if (type2->isEqual(unlabeled2)) {
          ++score2;
          continue;
        }
      }

      identical = false;
      continue;
    }

    // The systems are not considered equivalent.
    identical = false;

    // If one type is convertible to of the other, but not vice-versa.
    type1Better = tc.isConvertibleTo(type1, type2, cs.DC);
    type2Better = tc.isConvertibleTo(type2, type1, cs.DC);
    if (type1Better || type2Better) {
      if (type1Better)
        ++score1;
      if (type2Better)
        ++score2;
      continue;
    }

    // A concrete type is better than an archetype.
    // FIXME: Total hack.
    if (type1->is<ArchetypeType>() != type2->is<ArchetypeType>()) {
      if (type1->is<ArchetypeType>())
        ++score2;
      else
        ++score1;
      continue;
    }
    
    // FIXME:
    // This terrible hack is in place to support equality comparisons of non-
    // equatable option types to 'nil'. Until we have a way to constrain a type
    // variable on "!Equatable", if all other aspects of the overload choices
    // are equal, favor the overload that does not require an implicit literal
    // argument conversion to 'nil'.
    // Post-1.0, we'll need to remove this hack in favor of richer constraint
    // declarations.
    if (!(score1 || score2)) {
      if (auto nominalType2 = type2->getNominalOrBoundGenericNominal()) {
        if ((nominalType2->getName() ==
             cs.TC.Context.Id_OptionalNilComparisonType)) {
          ++score1;
        }
      } else if (auto nominalType1 = type1->getNominalOrBoundGenericNominal()) {
        if ((nominalType1->getName() ==
             cs.TC.Context.Id_OptionalNilComparisonType)) {
          ++score2;
        }
      }
    }
  }
  
  // All other things considered equal, if any overload choice is more
  // more constrained than the other, increment the score.
  if (score1 == score2) {
    if (foundRefinement1) {
      ++score1;
    }
    if (foundRefinement2) {
      ++score2;
    }
  }

  // FIXME: All other things being equal, prefer the catamorphism (flattening)
  // overload of ?? over the mplus (non-flattening) overload.
  if (score1 == score2) {
    // This is correct: we want to /disprefer/ the mplus.
    score2 += isStdlibOptionalMPlusOperator1;
    score1 += isStdlibOptionalMPlusOperator2;
  }

  // FIXME: There are type variables and overloads not common to both solutions
  // that haven't been considered. They make the systems different, but don't
  // affect ranking. We need to handle this.

  // If the scores are different, we have a winner.
  if (score1 != score2) {
    return score1 > score2? SolutionCompareResult::Better
                          : SolutionCompareResult::Worse;
  }

  // Neither system wins; report whether they were identical or not.
  return identical? SolutionCompareResult::Identical
                  : SolutionCompareResult::Incomparable;
}
Ejemplo n.º 10
0
AssignmentFailure::AssignmentFailure(Expr *destExpr, ConstraintSystem &cs,
                                     SourceLoc diagnosticLoc)
    : FailureDiagnostic(destExpr, cs, cs.getConstraintLocator(destExpr)),
      Loc(diagnosticLoc),
      DeclDiagnostic(findDeclDiagonstic(cs.getASTContext(), destExpr)),
      TypeDiagnostic(diag::assignment_lhs_not_lvalue) {}
/**
 * \brief Loop-Funktion der Testumgebung
 *
 * Diese Funktion wird direkt von display() aufgerufen. Hier
 * sollte alles reingeschrieben werden, was für die einzelnen
 * Tests nötig ist.
 */
TEFUNC void displayLoop() {
	const std::map<Id, RigidBodyPtr >* list = rbs.getMap();

	//Clock *zeit = new Clock();
	float interval = getLastTime();

	zeit += interval;

	if (doConstraints) {
		try {
			cs.step();
		} catch(std::exception& e) {
			std::cout << e.what() << std::endl;
		}
	}
	
	if (integrate) {
		//rbs.integrateEuler(interval);
		//rbs.integrateEuler(16);
                //rbs.integrateRungeKutta (20);
		rbs.integrateRungeKutta (10);
                //rbs.integrateVerletBaltman (10);
                //rbs.integrateEuler (10);
		if (postStabilization)
			cs.computePostStabilization ();
		//cout << interval << endl;
		rbs.addGravity();
		mrb->addForce(-1.0 * SimonState::exemplar()->getGravityVector()* mrb->getMass());
		
		if (doTorque) {
			mrb->addTorque(Vec3(20000.0,0.0,0.0));
		}
	}

	bool color = true;
	//bool posi = true;
	for (std::map<Id, SmartPointer<RigidBody> >::const_iterator p = list->begin(); p != list->end(); ++p) {
        
		Quaternion q = p->second->getOrientation();
		Vector3<float> axis;
		float angle;
		q.getAxisAngle(axis, angle);
		angle = RADTODEG(angle);
		Vector3<float> pos = p->second->getPosition();
		
		glLineWidth(1);
		
		glPushMatrix();
			glTranslatef(pos[0], pos[1], pos[2]);
			glRotatef(angle, axis[0], axis[1], axis[2]);
			
			
			
			if (color) {
				glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, Graphics::red);
				color=false;
			} else {
				glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, Graphics::green);
				
			}
			glutSolidCube(60.0);
				
				glPushMatrix();
				glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, Graphics::yellow);
				glTranslatef(-30.0,0.0,0.0);
				glutSolidSphere(10.0,5,5);
				glPopMatrix();
				glPushMatrix();
					glBegin(GL_LINES);
						glVertex3f(-30,0,0);
						glVertex3f(-30,0,90);
					glEnd();	
				glPopMatrix();
				
				glPushMatrix();
				glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, Graphics::blue);
				glTranslatef(30.0,0.0,0.0);
				glutSolidSphere(10.0,5,5);
				glPopMatrix();
				glPushMatrix();
					glBegin(GL_LINES);
						glVertex3f(30,0,0);
						glVertex3f(30,0,90);
					glEnd();	
				glPopMatrix();
				
			
		glPopMatrix();		
		
	}

	//sleep (1.0);
  
}
/**
 * \brief Start-Funktion der Testumgebung
 *
 * Diese Funktion wird einmal beim Starten der Testumgebung 
 * aufgerufen. Hier sollte alles reingeschrieben werden, 
 * was für die initialisierung der einzelnen Tests nötig ist.
 */
TEFUNC void initialize(int /*argc*/, char** /*argv*/) {
	Id id1;
	id1.setType(Id::typeBox);
	id1.setNumber(1);
	SmartPointer<RigidBody> rb = rbs.create(id1);
	
	rb->setMass(200000);
	rb->setPosition(Vector3<float>(30.0, 0.0, 0.0));
	rb->setPrevPosition(Vector3<float>(30.0, 0.0, 0.0));
	
	
	rb->setVelocity(Vector3<float>(0.00, 0.00, 0.00));
	
	//rb->setForce(Vector3<float>(0.000, 0.00, 0.00));
	rb->setForce(Vector3<float>(0.0, 0.00,0.00));
	
    
	
	Quaternion q = Quaternion();
	//q.normalize();
	rb->setOrientation(q);
	rb->setAngularVelocity(Vector3<float>(0.0, 0.0, 0.0));
	rb->setInertiaTensor((rb->getMass()/12) * 7200, (rb->getMass()/12) * 7200, (rb->getMass()/12) * 7200);
	
	//rb->setTorque(Vector3<float> (0.0, 0.00, -0.001));
 	rb->setTorque(Vector3<float> (0.0, 0.0, 0.0));	
	

	Id id2;
	id2.setType(Id::typeBox);
	id2.setNumber(2);
	SmartPointer<RigidBody> rb2 = rbs.create(id2);
	
	rb2->setMass(200);
	rb2->setPosition(Vector3<float>(-30.0, 0.0, 0.0));
	rb2->setPrevPosition(Vector3<float>(-30.0, 0.0, 0.0));
	rb2->setVelocity(Vector3<float>(0.0, 0.00, 0.00));
	
	//rb2->setForce(Vector3<float>(0.0,0.0, 0.0));
	rb2->setForce(Vector3<float>(0.0,0.0,0.0));
    
	Quaternion q2 = Quaternion();
	//q2.normalize();
	rb2->setOrientation(q2);
	rb2->setAngularVelocity(Vector3<float>(0.0, 0.0, 0.0));
	rb2->setInertiaTensor((rb2->getMass()/12) * 7200, (rb2->getMass()/12) * 7200, (rb2->getMass()/12) * 7200);
	
	//rb2->setTorque(Vector3<float> (0.0, 0.0, 0.001));
	rb2->setTorque(Vector3<float> (0.0, 0.0,0.0));
	
	Id id3;
	id3.setType(Id::typeBox);
	id3.setNumber(3);
	SmartPointer<RigidBody> rb3 = rbs.create(id3);
	
	rb3->setMass(200);
	rb3->setPosition(Vector3<float>(-90.0, 0.0, 0.0));
	rb3->setPrevPosition(Vector3<float>(-90.0, 0.0, 0.0));
	rb3->setVelocity(Vector3<float>(0.0, 0.00, 0.00));
	
	//rb3->setForce(Vector3<float>(0.0,0.0, 0.0));
	rb3->setForce(Vector3<float>(0.0,0.0,0.0));
    
	Quaternion q3 = Quaternion();
	//q2.normalize();
	rb3->setOrientation(q3);
	rb3->setAngularVelocity(Vector3<float>(0.0, 0.0, 0.0));
	rb3->setInertiaTensor((rb3->getMass()/12) * 7200, (rb3->getMass()/12) * 7200, (rb3->getMass()/12) * 7200);
	
	//rb3->setTorque(Vector3<float> (0.0, 0.0, 0.001));
	rb3->setTorque(Vector3<float> (0.0, 0.0,0.0));
	
	Id cid1;
	cid1.setType(Id::typeHingeJoint);
	cid1.setNumber(1);
	Id cid2;
	cid2.setType(Id::typeBallJoint);
	cid2.setNumber(2);
	
	Vector3 <float>  eq(-30, 0, 0);
	Vector3 <float> eq2(30, 0, 0);
	//Vector3 <float>  eu(-30, 90, 0);
	Vector3 <float>  eu(0, 0, 30);
        //Vector3 <float>  eu(10, 0, 0.0000000000000000000285);
        //Vector3 <float>  eu(10, 0, 0);
	//Vector3 <float>  eu2(30, 90, 0);
	Vector3 <float>  eu2(0, 0, 30);
	
	//cs.createBallAndSocketConstraint(cid1, rb, rb2, eq, eq2);
	cs.createHingeConstraint(cid1, rb, rb2, eq, eq2, eu, eu2);
	//cs.createBallAndSocketConstraint(cid2, rb2, rb3, eq, eq2);
	cs.createHingeConstraint(cid2, rb2, rb3, eq, eq2, eu, eu2);
	//mc1 = cs.getHingeConstraint(cid1);
	
	
	
	//cs.createBallAndSocketConstraint(cid1, rb, rb2, eq, eq2);
	cs.buildGraphs();
	
	mrb = rb;
	mrb2 = rb2;
	
	//mc1 = cs.getBallAndSocketConstraint(cid1);
	//mc2 = cs.getBallAndSocketConstraint(cid2);
	
	cs.printGraphs();
	cs.setComputationAlgorithm(1);
	cs.setTau(60);
	
 	SimonState::exemplar()->setViscositySlowdownAngular(0.7);
 	SimonState::exemplar()->setViscositySlowdownLinear(0.97);
	
	/*mc1->computeConstraint();
	
	cout << "Constraint:" << endl;
	mc1->getConstraint().show();
	cout << endl;
	cout << "ConstraintDot:" << endl;
	mc1->getConstraintDot().show();*/
	
	//SimonState::exemplar()->setGravityVector(Vec3(0,-0.00181,0));
	
}