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;
}
}
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;
}
// 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('
// 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(), ")!");
}
}
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;
}
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;
}
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));
}
