bool Event::isInfoEqual(const std::string& Key, const DoubleValue& Value) const { DoubleValue TmpValue; return (getInfoAsDoubleValue(Key,TmpValue) && openfluid::scientific::isCloseEnough(TmpValue.get(),Value.get(),0.00001)); }
/** * If getObservedValue() is NULL, then this method does nothing. * If there is an observedValue set AND the value is a subtype of * DoubleValue or IntValue, then the content of this value is set. * * Users may override this default implementation in subclasses to obtain * a different calculation behavior. The default implementation is only * there for convendience, as this way of usage is very common. * * @param index the index to write the calculated values to. * @return true if successful, false if there was an observed value with incompatible type. */ bool StatisticCalculator::calculateNextValue(int index) { if(mObservedValue == 0) { return true; } DoubleValue *doubleValue = dynamic_cast<DoubleValue*>(mObservedValue); if(doubleValue != 0) { setValue(index, doubleValue->get()); return true; } IntValue *intValue = dynamic_cast<IntValue*>(mObservedValue); if(intValue != 0) { setValue(index, intValue->get()); return true; } return false; }
double DynamicsPlotterUtil::getMeanValue(const QList<DoubleValue*> &valuesList) { if(valuesList.isEmpty()) { reportProblem("DynamicsPlotterUtil::getMeanValue : Empty list. Nothing to do."); return 0; } int nrValues = valuesList.size(); double meanValue = 0; for(int currVal = 0; currVal < nrValues; ++currVal) { DoubleValue* dVal = valuesList.at(currVal); if(dVal == 0) { reportProblem("DynamicsPlotterUtil::getMeanValue : Encountered NULL element."); return 0; } meanValue += dVal->get(); } return meanValue / nrValues; }
/** * Updates the history with the current value of the observed Value object. * Does nothing, if the history is set to static or if no Value object is set. */ void ValuePlotterItem::update() { if(mValue == 0 || mHasStaticHistory) { return; } double currentValue = 0.0; { //if DoubleValue DoubleValue *dv = dynamic_cast<DoubleValue*>(mValue); if(dv != 0) { currentValue = dv->get(); } else { IntValue *iv = dynamic_cast<IntValue*>(mValue); if(iv != 0) { currentValue = (double) iv->get(); } else { qDebug("PlotterItem: Value was not DoubleValue or IntValue!"); } } } addToHistory(currentValue); return; }
void BasinPlotter::calculateData() { // get program core Core *core = Core::getInstance(); // get network ModularNeuralNetwork *network = getCurrentNetwork(); if(network == 0) { Core::log("BasinPlotter: Could not find a neural network to work with! Aborting.", true); return; } QList<NeuralNetworkElement*> networkElements; network->getNetworkElements(networkElements); QList<DoubleValue*> networkValues = DynamicsPlotterUtil::getNetworkValues(networkElements); // Get parameters for varied elements QString variedX = mVariedX->get(); QString variedY = mVariedY->get(); if(variedX.isEmpty() || variedY.isEmpty()) { reportProblem("BasinPlotter: No elements to vary."); return; } DoubleValue *variedValX = DynamicsPlotterUtil::getElementValue( variedX, networkElements, &mNeuronsWithActivationsToTransfer); DoubleValue *variedValY = DynamicsPlotterUtil::getElementValue( variedY, networkElements, &mNeuronsWithActivationsToTransfer); if(variedValX == 0 || variedValY == 0) { reportProblem("BasinPlotter: NULL pointer for varied element. Aborting."); return; } QList<double> variedRangeX = DynamicsPlotterUtil::getDoublesFromString(mVariedRangeX->get()); QList<double> variedRangeY = DynamicsPlotterUtil::getDoublesFromString(mVariedRangeY->get()); if(variedRangeX.size() != 2 || variedRangeY.size() != 2) { reportProblem("BasinPlotter: Not a valid range given."); return; } int resolutionX = mResolutionX->get(); int resolutionY = mResolutionY->get(); //avoid division by zero! if(resolutionX < 2 || resolutionY < 2) { reportProblem("BasinPlotter: Invalid resolution given."); return; } // projected elements int nrProjections = 0; QString projectionsX = mProjectionsX->get(); QString projectionsY = mProjectionsY->get(); QList< QList<DoubleValue*> > projectionValuesX; QList< QList<DoubleValue*> > projectionValuesY; QList<double> projectionRangesX; QList<double> projectionRangesY; if(projectionsX != "0" && projectionsY != "0") { QList<QStringList> projectionListX = DynamicsPlotterUtil::parseElementString(projectionsX); QList<QStringList> projectionListY = DynamicsPlotterUtil::parseElementString(projectionsY); projectionValuesX = DynamicsPlotterUtil::getElementValues(projectionListX, networkElements); projectionValuesY = DynamicsPlotterUtil::getElementValues(projectionListY, networkElements); if(projectionValuesX.isEmpty() || projectionValuesY.isEmpty()) { reportProblem("BasinPlotter: Could not find specified elements to project onto."); return; } if(projectionValuesX.size() != projectionValuesY.size()) { reportProblem("BasinPlotter: Mismatching number of projected elements for the two axes."); return; } projectionRangesX = DynamicsPlotterUtil::getDoublesFromString(mProjectionRangesX->get()); projectionRangesY = DynamicsPlotterUtil::getDoublesFromString(mProjectionRangesY->get()); if(projectionRangesX.size() != 2*projectionValuesX.size() || projectionRangesY.size() != 2*projectionValuesY.size()) { reportProblem("BasinPlotter: Given ranges for projection don't match number of elements."); return; } nrProjections = projectionValuesX.size(); } // save original values for clean-up QList<double> variedValuesOrig; variedValuesOrig.append(QList<double>() << variedValX->get() << variedValY->get()); bool resetNetworkActivation = mResetNetworkActivation->get(); storeCurrentNetworkActivities(); /* store network configuration (bias terms, synapse weights, observable parameters of TFs, AFs, SFs. */ bool restoreNetConfiguration = mRestoreNetworkConfiguration->get(); storeNetworkConfiguration(); //This is important when the physical simulator is activated! bool resetSimulation = mResetSimulator->get(); triggerReset(); // PREPARE data matrix double xStart = variedRangeX.first(); double xEnd = variedRangeX.last(); double xStepSize = (xEnd - xStart) / (double) (resolutionX - 1); int roundDigits = mRoundDigits->get(); double xVal; QList<double> xValues; double yStart = variedRangeY.first(); double yEnd = variedRangeY.last(); double yStepSize = (yEnd - yStart) / (double) (resolutionY - 1); double yVal; QList<double> yValues; { //Thread safety of matrix. QMutexLocker guard(mDynamicsPlotManager->getMatrixLocker()); mData->clear(); mData->resize(resolutionX + 1, resolutionY + 1, 3 + nrProjections); mData->fill(0); // calculate values and draw axes for(int x = 1; x <= resolutionX; ++x) { xVal = xStart + (x - 1) * xStepSize; mData->set(Math::round(xVal, 5), x, 0, 0); mData->set(Math::round(xVal, 5), x, 0, 1); mData->set(Math::round(xVal, 5), x, 0, 2); if(roundDigits >= 0) { xVal = Math::round(xVal, roundDigits); } xValues.append(xVal); } for(int y = 1; y <= resolutionY; ++y) { yVal = yStart + (y - 1) * yStepSize; mData->set(Math::round(yVal, 5), 0, y, 0); mData->set(Math::round(yVal, 5), 0, y, 1); mData->set(Math::round(yVal, 5), 0, y, 2); if(roundDigits >= 0) { yVal = Math::round(yVal, roundDigits); } yValues.append(yVal); } // same for additional projections for(int currProj = 0; currProj < nrProjections; ++currProj) { double pStartX = projectionRangesX.at(currProj * 2); double pEndX = projectionRangesX.at(currProj * 2 + 1); double pStepX = (pEndX - pStartX) / (double) (resolutionX - 1); for(int x = 1; x <= resolutionX; ++x) { mData->set(Math::round((pStartX + (x - 1) * pStepX), 5), x, 0, 3 + currProj); } double pStartY = projectionRangesY.at(currProj * 2); double pEndY = projectionRangesY.at(currProj * 2 + 1); double pStepY = (pEndY - pStartY) / (double) (resolutionY - 1); for(int y = 1; y <= resolutionY; ++y) { mData->set(Math::round((pStartY + (y - 1) * pStepY), 5), 0, y, 3 + currProj); } } } // MAIN LOOP over x parameter points int stepsRun = mStepsToRun->get(); int stepsCheck = mStepsToCheck->get(); double accuracy = mAccuracy->get(); QList< QList<double> > attractors; for(int x = 1; x <= resolutionX && mActiveValue->get(); ++x) { mProgressPercentage->set((double)(100 * x / resolutionX)); // INNER LOOP over y parameter points for(int y = 1; y <= resolutionY && mActiveValue->get(); ++y) { if(resetSimulation) { triggerReset(); } if(restoreNetConfiguration) { restoreNetworkConfiguration(); } if(resetNetworkActivation) { restoreCurrentNetworkActivites(); } // set x parameter variedValX->set(xValues.at(x - 1)); // set y parameter variedValY->set(yValues.at(y - 1)); if(!notifyNetworkParametersChanged(network)) { return; } for(int runStep = 0; runStep < stepsRun && mActiveValue->get(); ++runStep) { // let the network run for 1 timestep triggerNetworkStep(); } QList< QList<double> > networkStates; QList<double> networkState; QList< QPair<double,double> > variedPositions; QList< QPair< QList<double>, QList<double> > > projectionPositions; bool foundMatch = false; int attrPeriod = 0; for(int checkStep = 0; checkStep <= stepsCheck && !foundMatch && mActiveValue->get(); ++checkStep) { triggerNetworkStep(); // get current network state networkState = DynamicsPlotterUtil::getNetworkState(networkValues); // abort on empty state if(networkState.isEmpty()) { reportProblem("BasinPlotter: Encountered empty network state."); return; } // compare states to find attractors for(int period = 1; period <= checkStep && !foundMatch; ++period) { foundMatch = DynamicsPlotterUtil::compareNetworkStates( networkStates.at(checkStep-period), networkState, accuracy); attrPeriod = period; } // save current state as last one networkStates.append(networkState); variedPositions.append(QPair<double,double>(variedValX->get(), variedValY->get())); if(nrProjections > 0) { QPair< QList<double>, QList<double> > currentPositions; currentPositions.first = DynamicsPlotterUtil::getMeanValues(projectionValuesX); currentPositions.second = DynamicsPlotterUtil::getMeanValues(projectionValuesY); projectionPositions.append(currentPositions); } } // at this point, either an attractor has been found if(foundMatch && mActiveValue->get()) { // check for past attractors bool attrMatch = false; int attrNo = 1; while(attrNo <= attractors.size() && !attrMatch) { for(int state = 1; state <= attrPeriod && !attrMatch; ++state) { attrMatch = DynamicsPlotterUtil::compareNetworkStates( attractors.at(attrNo-1), networkStates.at(networkStates.size()-state), // was: size()-1-state accuracy); } attrNo++; } //Thread safety of matrix. QMutexLocker guard(mDynamicsPlotManager->getMatrixLocker()); // write matrix mData->set(attrNo, x, y, 0); mData->set(attrPeriod, x, y, 1); // calculate and plot attractor position int nrPositions = variedPositions.size(); for(int periodPos = 1; periodPos <= attrPeriod; ++periodPos) { int currPosition = nrPositions - periodPos; double currValX = variedPositions.at(currPosition).first; double currValY = variedPositions.at(currPosition).second; int attrPosX = ceil((currValX - xStart) / xStepSize + 1); int attrPosY = ceil((currValY - yStart) / yStepSize + 1); mData->set(attrNo, attrPosX, attrPosY, 2); for(int currProj = 0; currProj < nrProjections; ++currProj) { double xVal = projectionPositions.at(currPosition).first.at(currProj); double yVal = projectionPositions.at(currPosition).second.at(currProj); double pStartX = projectionRangesX.at(currProj * 2); double pEndX = projectionRangesX.at(currProj * 2 + 1); double pStepX = (pEndX - pStartX) / (double) (resolutionX - 1); double pStartY = projectionRangesY.at(currProj * 2); double pEndY = projectionRangesY.at(currProj * 2 + 1); double pStepY = (pEndY - pStartY) / (double) (resolutionY - 1); int xPos = floor((xVal - pStartX) / pStepX + 1); int yPos = floor((yVal - pStartY) / pStepY + 1); mData->set(attrNo, xPos, yPos, 3 + currProj); } } if(!attrMatch) { attractors.append(networkStates.last()); } } // or not, but then there's nothing to do :D // runtime maintencance if(core->isShuttingDown()) { return; } core->executePendingTasks(); } } // CLEAN UP variedValX->set(variedValuesOrig.at(0)); variedValY->set(variedValuesOrig.at(1)); notifyNetworkParametersChanged(network); triggerReset(); restoreNetworkConfiguration(); restoreCurrentNetworkActivites(); }
void AccelSensor::setup() { SimObject::setup(); if(mInitialized == false) { mHostBody = Physics::getPhysicsManager()->getSimBody(mReferenceBodyName->get()); if(mHostBody == 0) { Core::log("AccelSensor: Sensor has no valid reference body. Name was [" + mReferenceBodyName->get() + "]! Ignoring", true); return; } createSensor(); mFirstSensorValue->set(0.0); mSecondSensorValue->set(0.0); mThirdSensorValue->set(0.0); mTimeStepSize = dynamic_cast<DoubleValue*>(Core::getInstance()-> getValueManager()->getValue(SimulationConstants::VALUE_TIME_STEP_SIZE)); DoubleValue *mGravitationValue = dynamic_cast<DoubleValue*>(Core::getInstance()-> getValueManager()->getValue("/Simulation/Gravitation")); if(mTimeStepSize == 0 || mGravitationValue == 0) { Core::log("AccelSensor: Required Event or Value could not be found."); return; } mGravitation = mGravitationValue->get(); mLastAxisOneMeasurement = 0.0; mLastAxisTwoMeasurement = 0.0; mLastAxisThreeMeasurement = 0.0; mInitialized = true; } // rotate chosen axis about the localRotation of this sensor Quaternion localOrientation = mLocalOrientation->get(); localOrientation.normalize(); mLocalOrientation->set(localOrientation); Quaternion localOrientationInverse = localOrientation.getInverse(); localOrientationInverse.normalize(); Quaternion xAxis(0.0, mSensorAxisOneValue->getX(), mSensorAxisOneValue->getY(), mSensorAxisOneValue->getZ()); Quaternion xAxisRotated = localOrientation * xAxis * localOrientationInverse; mSensorAxisOne.set(xAxisRotated.getX(), xAxisRotated.getY(), xAxisRotated.getZ()); mRotatedSensorAxisOne.set(xAxisRotated.getX(), xAxisRotated.getY(), xAxisRotated.getZ()); Quaternion yAxis(0.0, mSensorAxisTwoValue->getX(), mSensorAxisTwoValue->getY(), mSensorAxisTwoValue->getZ()); Quaternion yAxisRotated = localOrientation * yAxis * localOrientationInverse; mSensorAxisTwo.set(yAxisRotated.getX(), yAxisRotated.getY(), yAxisRotated.getZ()); mRotatedSensorAxisTwo.set(yAxisRotated.getX(), yAxisRotated.getY(), yAxisRotated.getZ()); Quaternion zAxis(0.0, mSensorAxisThreeValue->getX(), mSensorAxisThreeValue->getY(), mSensorAxisThreeValue->getZ()); Quaternion zAxisRotated = localOrientation * zAxis * localOrientationInverse; mSensorAxisThree.set(zAxisRotated.getX(), zAxisRotated.getY(), zAxisRotated.getZ()); mRotatedSensorAxisThree.set(zAxisRotated.getX(), zAxisRotated.getY(), zAxisRotated.getZ()); if(mSensorBody == 0 || mHostBody == 0) { Core::log("AccelSensor: The sensor has no valid host or sensor-body."); return; } mSensorBody->setTextureType("None"); mSensorBody->setFaceTexture(5, "AccelBoard"); mSensorGeometry->setLocalOrientation(mLocalOrientation->get()); mSensorBody->getGeometry()->setLocalPosition(mLocalPosition->get()); Quaternion localPos(0, mLocalPosition->getX(), mLocalPosition->getY(), mLocalPosition->getZ()); Quaternion bodyOrientationInverse = mHostBody->getQuaternionOrientationValue()->get().getInverse(); Quaternion rotatedLocalPosQuat = mHostBody->getQuaternionOrientationValue()->get() * localPos * bodyOrientationInverse; Vector3D rotatedLocalPos(rotatedLocalPosQuat.getX(), rotatedLocalPosQuat.getY(), rotatedLocalPosQuat.getZ()); mLastPosition = mHostBody->getPositionValue()->get() + rotatedLocalPos; if(mLowPassFilterDelayValue != 0) { mLowPassFilterDelay = mLowPassFilterDelayValue->get(); } mValueOneHistory.clear(); mValueTwoHistory.clear(); mValueThreeHistory.clear(); mGlobalNoiseDeviation = mGlobalNoiseDeviationValue->get(); }
void LyapunovExponent::calculateData() { // get program core Core *core = Core::getInstance(); // get network ModularNeuralNetwork *network = getCurrentNetwork(); QList<NeuralNetworkElement*> networkElements; network->getNetworkElements(networkElements); QList<DoubleValue*> networkValues = DynamicsPlotterUtil::getNetworkValues(networkElements); // Get parameters for varied element QString variedElement = mVariedElement->get(); if(variedElement.isEmpty()) { reportProblem("LyapunovExponent: No element to vary."); return; } DoubleValue *variedValue = DynamicsPlotterUtil::getElementValue(variedElement, networkElements); if(variedValue == 0) { reportProblem("LyapunovExponent: Invalid value or specifier."); return; } QList<double> variedRange = DynamicsPlotterUtil::getDoublesFromString(mVariedRange->get()); if(variedRange.size() != 2) { reportProblem("LyapunovExponent: Invalid parameter range."); return; } int resolutionX = mResolutionX->get(); int resolutionY = mResolutionY->get(); //avoid division by zero! if(resolutionX < 2 || resolutionY < 2) { reportProblem("LyapunovExponent: Invalid resolution given."); return; } // Let costraint resolver run properly (order matters!) storeNetworkConfiguration(); storeCurrentNetworkActivities(); triggerReset(); restoreCurrentNetworkActivites(); restoreNetworkConfiguration(); notifyNetworkParametersChanged(network); // save original value double originalValue = variedValue->get(); double valStep = (variedRange.at(1) - variedRange.at(0)) / (double) (resolutionX - 1); QList<double> variedValues; // prepare data matrix { //Thread safety of matrix. QMutexLocker guard(mDynamicsPlotManager->getMatrixLocker()); mData->clear(); mData->resize(resolutionX + 1, resolutionY + 1, 1); mData->fill(0); for(int x = 1; x <= resolutionX; ++x) { double val = variedRange.at(0) + (x-1) * valStep; variedValues.append(val); mData->set(val, x, 0, 0); } } int stepsPrePlot = mStepsPrePlot->get(); int stepsToPlot = mStepsToPlot->get(); bool drawNL = mDrawNL->get(); QList<double> ynum; double eps = pow(10,-9); for(int x = 0; x < variedValues.size(); ++x) { // set initial conditions of this run/trajectory variedValue->set(variedValues.at(x)); notifyNetworkParametersChanged(network); // calculate activation after X PrePlot-Steps for(int s = 0; s < stepsPrePlot && mActiveValue->get(); ++s) { triggerNetworkStep(); } // list for states QList< QList<double> > networkStates; for(int s = 0; s < stepsToPlot && mActiveValue->get(); ++s) { triggerNetworkStep(); // get current state of the network QList<double> networkState = DynamicsPlotterUtil::getNetworkState(networkValues); // save to list networkStates.append(networkState); } double ljanum = 0; int c = 0; for(int i = 0; i < networkStates.size() - 1; ++i) { double dy = 10000000, df = 100000000; bool found = false; for(int j = 0; j < networkStates.size() - 1; ++j) { double d = DynamicsPlotterUtil::getDistance( networkStates.at(i), networkStates.at(j)); if(d < dy && d > eps) { dy = d; df = DynamicsPlotterUtil::getDistance( networkStates.at(i + 1), networkStates.at(j + 1)); found = true; } } if(found && dy != 0 && df != 0) { ljanum += log(df / dy); c++; } } // save current hightest exponent ynum.append(ljanum / c); // find smallest and biggest exponent double ymin = ynum.first(); double ymax = ynum.first(); for(int i = 1; i < ynum.size(); ++i) { double y = ynum.at(i); if(y < ymin) { ymin = y; } if(y > ymax) { ymax = y; } } double ystep = (ymax - ymin) / (double)(resolutionY - 1); if(ystep == 0) { reportProblem("LyapunovExponent: No suitable data found."); ymin = 1; ymax = 1; } { //Thread safety of matrix. QMutexLocker guard(mDynamicsPlotManager->getMatrixLocker()); // clear data matrix mData->fill(0); // rescale for(int y = 1; y <= resolutionY; ++y) { double v = ymin + (y-1) * ystep; mData->set(Math::round(v, 5), 0, y, 0); } // fill rescaled matrix again for(int x = 1; x <= ynum.size(); ++x) { double v = min(max(ymin, ynum.at(x - 1)), ymax); int y = ceil(((v - ymin) / ystep) + 1); mData->set(1, x, y, 0); } // find null position (if any) int ny = ceil(((-ymin)/ystep)+1); // and draw red line indicating y=0 if(drawNL && ny < resolutionY && ny > 0) { for(int x = 0; x < resolutionX; ++x) { if(mData->get(x, ny, 0) == 0) { mData->set(2, x, ny, 0); } } } } // runtime maintencance if(core->isShuttingDown()) { return; } core->executePendingTasks(); } // re-set original parameter value variedValue->set(originalValue); // CLEAN UP notifyNetworkParametersChanged(network); triggerReset(); restoreNetworkConfiguration(); restoreCurrentNetworkActivites(); }
dJointID ODE_PID_PassiveActuatorModel::createJoint(dBodyID body1, dBodyID body2) { Vector3DValue *jointAxisPoint1 = dynamic_cast<Vector3DValue*>(mOwner->getParameter("AxisPoint1")); Vector3DValue *jointAxisPoint2 = dynamic_cast<Vector3DValue*>(mOwner->getParameter("AxisPoint2")); DoubleValue *minAngleValue = dynamic_cast<DoubleValue*>(mOwner->getParameter("MinAngle")); DoubleValue *maxAngleValue = dynamic_cast<DoubleValue*>(mOwner->getParameter("MaxAngle")); if(jointAxisPoint1 == 0 || jointAxisPoint2 == 0 || minAngleValue == 0 || maxAngleValue == 0) { Core::log("ODE_PID_PassiveActuatorModel: Could not find all required parameter values."); return 0; } if(jointAxisPoint1->get().equals(jointAxisPoint2->get(), -1)) { Core::log("ODE_PID_PassiveActuatorModel: Invalid axis points " + jointAxisPoint1->getValueAsString() + " and " + jointAxisPoint2->getValueAsString() + "."); return 0; } if(jointAxisPoint1->get().equals(jointAxisPoint2->get(), -1)) { Core::log("Invalid axes for ODE_PID_PassiveActuatorModel."); return 0; } Vector3D anchor = jointAxisPoint1->get(); Vector3D axis = jointAxisPoint2->get() - jointAxisPoint1->get(); dJointID joint = dJointCreateAMotor(mWorldID, mGeneralJointGroup); dJointAttach(joint, body1, body2); dJointSetAMotorMode(joint, dAMotorEuler); dJointSetAMotorParam(joint, dParamVel, 0.0); dJointSetAMotorParam(joint, dParamFMax, 1.0); dJointSetAMotorParam(joint, dParamCFM, mCFMValue->get()); dJointSetAMotorParam(joint, dParamStopERP, mStopERPValue->get()); dJointSetAMotorParam(joint, dParamStopCFM, mStopCFMValue->get()); dJointSetAMotorParam(joint, dParamBounce, mBounceValue->get()); dJointSetAMotorParam(joint, dParamFudgeFactor, mFudgeFactorValue->get()); axis.normalize(); Vector3D perpedicular; if(axis.getY() != 0.0 || axis.getX() != 0.0) { perpedicular.set(-axis.getY(), axis.getX(), 0); } else { perpedicular.set(0, -axis.getZ(), axis.getY()); } perpedicular.normalize(); // If one of the bodies is static, the motor axis need to be defined in a different way. For different constellations of static and dynamic bodies, the turn direction of the motor changed, so this had to be added. if(body1 == 0) { dJointSetAMotorAxis(joint, 0, 0, -axis.getX(), -axis.getY(), -axis.getZ()); } else { dJointSetAMotorAxis(joint, 0, 1, axis.getX(), axis.getY(), axis.getZ()); } if(body2 == 0) { dJointSetAMotorAxis(joint, 2, 0, -perpedicular.getX(), -perpedicular.getY(), -perpedicular.getZ()); } else { dJointSetAMotorAxis(joint, 2, 2, perpedicular.getX(), perpedicular.getY(), perpedicular.getZ()); } mHingeJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup); dJointAttach(mHingeJoint, body1, body2); dJointSetHingeAnchor(mHingeJoint, anchor.getX(), anchor.getY(), anchor.getZ()); dJointSetHingeAxis(mHingeJoint, axis.getX(), axis.getY(), axis.getZ()); double minAngle = (minAngleValue->get() * Math::PI) / 180.0; double maxAngle = (maxAngleValue->get() * Math::PI) / 180.0; if(body1 == 0) { double tmp = minAngle; minAngle = -1.0 * maxAngle; maxAngle = -1.0 * tmp; } dJointSetHingeParam(mHingeJoint, dParamLoStop, minAngle); dJointSetHingeParam(mHingeJoint, dParamHiStop, maxAngle); dJointSetHingeParam(mHingeJoint, dParamVel, 0.0); return joint; }
void TestSignals::testSignalSinus() { Core::resetCore(); // Test SinusSignal // Create the TimeStepSizeValue DoubleValue *timeStepSizeValue = new DoubleValue(0.01); Core::getInstance()->getValueManager()->addValue(SimulationConstants::VALUE_TIME_STEP_SIZE, timeStepSizeValue); // Create the CurrentStepValue IntValue *currentStepValue = new IntValue(0); Core::getInstance()->getValueManager()->addValue(SimulationConstants::VALUE_EXECUTION_CURRENT_STEP, currentStepValue); // Create the NextStepEvent Event* nextStepEvent = Core::getInstance()->getEventManager()->createEvent(NerdConstants::EVENT_EXECUTION_NEXT_STEP); QVERIFY(nextStepEvent != 0); // Create the signal and initialize the core SignalSinus* mySignalSinus = new SignalSinus("MySinus", "/Signals"); Core::getInstance()->init(); // CurrentValue und IdleSteps Parameter should be there DoubleValue* currentValue = dynamic_cast<DoubleValue*>(mySignalSinus->getParameter("CurrentValue")); QVERIFY(currentValue != 0); IntValue* idleSteps = dynamic_cast<IntValue*>(mySignalSinus->getParameter("IdleSteps")); QVERIFY(idleSteps != 0); QVERIFY(currentValue->get() == 0.0); QVERIFY(idleSteps->get() == 0); // Current value should be 0.0 QVERIFY(mySignalSinus->getCurrentValue() == 0.0); // Test getName() QVERIFY(mySignalSinus->getName() == "MySinus"); // Period, Amplitude, Offset and AngularPhaseShift Paramaters should be there DoubleValue* period = dynamic_cast<DoubleValue*>(mySignalSinus->getParameter("Period")); QVERIFY(period != 0); QVERIFY(period->get() == 1.0); DoubleValue* amplitude = dynamic_cast<DoubleValue*>(mySignalSinus->getParameter("Amplitude")); QVERIFY(amplitude != 0); QVERIFY(amplitude->get() == 1.0); DoubleValue* offset = dynamic_cast<DoubleValue*>(mySignalSinus->getParameter("Offset")); QVERIFY(offset != 0); QVERIFY(offset->get() == 0.0); DoubleValue* phase = dynamic_cast<DoubleValue*>(mySignalSinus->getParameter("Phase")); QVERIFY(phase != 0); QVERIFY(phase->get() == 0.0); // Test computation double desiredValue; currentStepValue->set(1); nextStepEvent->trigger(); desiredValue = Math::sin(1.0 / 100.0 * 2 * Math::PI); QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10)); currentStepValue->set(30); nextStepEvent->trigger(); desiredValue = Math::sin(30.0 / 100.0 * 2 * Math::PI); QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10)); timeStepSizeValue->set(0.02); nextStepEvent->trigger(); desiredValue = Math::sin(2.0 * 30.0 / 100.0 * 2 * Math::PI); QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10)); idleSteps->set(10); nextStepEvent->trigger(); desiredValue = Math::sin(2.0 * 20.0 / 100.0 * 2 * Math::PI); QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10)); period->set(1.5); nextStepEvent->trigger(); desiredValue = Math::sin((30 - 10) / (1.5 / 0.02) * 2 * Math::PI); QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10)); amplitude->set(3.3); nextStepEvent->trigger(); desiredValue = 3.3 * Math::sin((30 - 10) / (1.5 / 0.02) * 2 * Math::PI); QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10)); offset->set(0.7); nextStepEvent->trigger(); desiredValue = 0.7 + 3.3 * Math::sin((30 - 10) / (1.5 / 0.02) * 2 * Math::PI); QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10)); phase->set(0.12); nextStepEvent->trigger(); desiredValue = 0.7 + 3.3 * Math::sin(0.12 + (30 - 10) / (1.5 / 0.02) * 2 * Math::PI); QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10)); // Test addDestinationValue mechanism DoubleValue *destValue1 = new DoubleValue(0.0); DoubleValue *destValue2 = new DoubleValue(0.0); DoubleValue *destValue3 = new DoubleValue(0.0); mySignalSinus->addDestinationValue(destValue1); mySignalSinus->addDestinationValue(destValue2); mySignalSinus->addDestinationValue(destValue3); period->set(1.0); amplitude->set(1.0); offset->set(0.0); phase->set(0.0); idleSteps->set(0); timeStepSizeValue->set(0.01); currentStepValue->set(1); nextStepEvent->trigger(); desiredValue = Math::sin(1.0 / 100.0 * 2 * Math::PI); QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10)); QCOMPARE(destValue1->get(), mySignalSinus->getCurrentValue()); QCOMPARE(destValue2->get(), mySignalSinus->getCurrentValue()); QCOMPARE(destValue3->get(), mySignalSinus->getCurrentValue()); // Test removeDestinationValue mechanism mySignalSinus->removeDestinationValue(destValue3); currentStepValue->set(2); nextStepEvent->trigger(); desiredValue = Math::sin(2.0 / 100.0 * 2 * Math::PI); QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10)); QCOMPARE(destValue1->get(), mySignalSinus->getCurrentValue()); QCOMPARE(destValue2->get(), mySignalSinus->getCurrentValue()); QVERIFY(destValue3->get() != mySignalSinus->getCurrentValue()); delete destValue1; delete destValue2; delete destValue3; Core::resetCore(); }
//chris void TestValueTransferController::testConstructor() { Core::resetCore(); ValueTransferController *vtf = new ValueTransferController("TFController"); QCOMPARE(vtf->getParameters().size(), 12); StringValue *nameOfSource = dynamic_cast<StringValue*>(vtf->getParameter("SourceValueName")); StringValue *nameOfTarget = dynamic_cast<StringValue*>(vtf->getParameter("TargetValueName")); StringValue *customControllerName = dynamic_cast<StringValue*>(vtf->getParameter("CustomControllerName")); StringValue *customSensorName = dynamic_cast<StringValue*>(vtf->getParameter("CustomSensorName")); InterfaceValue *controller = dynamic_cast<InterfaceValue*>(vtf->getParameter("Control")); InterfaceValue *sensor = dynamic_cast<InterfaceValue*>(vtf->getParameter("Sensor")); IntValue *mode = dynamic_cast<IntValue*>(vtf->getParameter("TransferMode")); DoubleValue *rate = dynamic_cast<DoubleValue*>(vtf->getParameter("TransferRate")); DoubleValue *cost = dynamic_cast<DoubleValue*>(vtf->getParameter("TransferCost")); QVERIFY(nameOfSource != 0); QVERIFY(nameOfTarget != 0); QVERIFY(customControllerName != 0); QVERIFY(customSensorName != 0); QVERIFY(controller != 0); QVERIFY(sensor != 0); QVERIFY(mode != 0); QVERIFY(rate != 0); QVERIFY(cost != 0); QCOMPARE(vtf->getOutputValues().size(), 1); QVERIFY(vtf->getOutputValues().contains(sensor)); QCOMPARE(vtf->getInputValues().size(), 1); QVERIFY(vtf->getInputValues().contains(controller)); QVERIFY(controller->getName() == "TFController/Control"); QVERIFY(sensor->getName() == "TFController/Transfer"); QCOMPARE(controller->getMin(), -1.0); QCOMPARE(controller->getMax(), 1.0); QCOMPARE(sensor->getMin(), -1.0); QCOMPARE(sensor->getMax(), 1.0); //change the custom names customControllerName->set("NewName"); QVERIFY(controller->getName() == "TFController/NewName"); customSensorName->set("NewSensorName"); QVERIFY(sensor->getName() == "TFController/NewSensorName"); //change some values nameOfSource->set("MySource"); nameOfTarget->set("MyTarget"); mode->set(10); rate->set(0.12345); cost->set(-5.123); //************************************************************************************* //copy constructor ValueTransferController *copy = dynamic_cast<ValueTransferController*>(vtf->createCopy()); QVERIFY(copy != 0); copy->setName("MyCopy"); StringValue *nameOfSourceC = dynamic_cast<StringValue*>(copy->getParameter("SourceValueName")); StringValue *nameOfTargetC = dynamic_cast<StringValue*>(copy->getParameter("TargetValueName")); StringValue *customControllerNameC = dynamic_cast<StringValue*>(copy->getParameter("CustomControllerName")); StringValue *customSensorNameC = dynamic_cast<StringValue*>(copy->getParameter("CustomSensorName")); InterfaceValue *controllerC = dynamic_cast<InterfaceValue*>(copy->getParameter("Control")); InterfaceValue *sensorC = dynamic_cast<InterfaceValue*>(copy->getParameter("Sensor")); IntValue *modeC = dynamic_cast<IntValue*>(copy->getParameter("TransferMode")); DoubleValue *rateC = dynamic_cast<DoubleValue*>(copy->getParameter("TransferRate")); DoubleValue *costC = dynamic_cast<DoubleValue*>(copy->getParameter("TransferCost")); QVERIFY(nameOfSourceC != 0); QVERIFY(nameOfTargetC != 0); QVERIFY(customControllerNameC != 0); QVERIFY(customSensorNameC != 0); QVERIFY(controllerC != 0); QVERIFY(sensorC != 0); QVERIFY(modeC != 0); QVERIFY(rateC != 0); QVERIFY(costC != 0); QVERIFY(vtf->getSource() == 0); QVERIFY(vtf->getTarget() == 0); QCOMPARE(copy->getOutputValues().size(), 1); QVERIFY(copy->getOutputValues().contains(sensorC)); QCOMPARE(copy->getInputValues().size(), 1); QVERIFY(copy->getInputValues().contains(controllerC)); QVERIFY(sensor != sensorC); QVERIFY(controller != controllerC); QVERIFY(controllerC->getName() == "MyCopy/NewName"); QVERIFY(sensorC->getName() == "MyCopy/NewSensorName"); QCOMPARE(modeC->get(), 10); QCOMPARE(rateC->get(), 0.12345); QCOMPARE(costC->get(), -5.123); QVERIFY(nameOfSourceC->get() == "MySource"); QVERIFY(nameOfTargetC->get() == "MyTarget"); QVERIFY(customControllerNameC->get() == "NewName"); QVERIFY(customSensorNameC->get() == "NewSensorName"); }
void TestSphereBody::testSphereBody() { Core::resetCore(); SphereBody *sphere = new SphereBody("Sphere"); QCOMPARE(sphere->getParameters().size(), 14); QVERIFY(sphere->getName().compare("Sphere") == 0); QVERIFY(sphere->getInputValues().size() == 0); QVERIFY(sphere->getOutputValues().size() == 0); QCOMPARE(sphere->getParameters().size(), 14); QVERIFY(sphere->getParameter("CenterOfMass") != 0); QVERIFY(sphere->getParameter("Position") != 0); QVERIFY(sphere->getParameter("Orientation") != 0); QVERIFY(sphere->getParameter("DynamicFriction") != 0); QVERIFY(sphere->getParameter("StaticFriction") != 0); QVERIFY(sphere->getParameter("Elasticity") != 0); QVERIFY(sphere->getParameter("Dynamic") != 0); QVERIFY(sphere->getParameter("Name") != 0); QVERIFY(sphere->getParameter("Mass") != 0); QVERIFY(sphere->getParameter("Color") != 0); DoubleValue *radius = dynamic_cast<DoubleValue*>(sphere->getParameter("Radius")); QVERIFY(radius != 0); QCOMPARE(radius->get(), 0.0); QCOMPARE(sphere->getCollisionObjects().size(), 1); QCOMPARE(sphere->getGeometries().size(), 1); SphereGeom *sphereGeom = dynamic_cast<SphereGeom*>(sphere->getGeometries().at(0)); QVERIFY(sphereGeom != 0); QCOMPARE(sphereGeom->getRadius(), 0.0); CollisionObject *collisionObject = sphere->getCollisionObjects().at(0); QVERIFY(collisionObject->getHostBody() == sphere); QVERIFY(collisionObject->getOwner() == 0); SphereGeom *collisionObjectGeom = dynamic_cast<SphereGeom*>(collisionObject->getGeometry()); QVERIFY(collisionObjectGeom != 0); QCOMPARE(collisionObjectGeom->getRadius(), 0.0); radius->set(1.0); QCOMPARE(collisionObjectGeom->getRadius(), 0.0); QCOMPARE(sphereGeom->getRadius(), 0.0); sphere->setup(); QVERIFY(collisionObject->getHostBody() == sphere); QVERIFY(collisionObject->getOwner() == sphere); QCOMPARE(collisionObjectGeom->getRadius(), 1.0); QCOMPARE(sphereGeom->getRadius(), 1.0); SphereBody *sphere2 = new SphereBody("Sphere2", 5.14); DoubleValue *radius2 = dynamic_cast<DoubleValue*>(sphere2->getParameter("Radius")); QVERIFY(radius2 != 0); QCOMPARE(radius2->get(), 5.14); SphereGeom *sphereGeom2 = dynamic_cast<SphereGeom*>(sphere2->getCollisionObjects().at(0)->getGeometry()); QCOMPARE(sphereGeom2->getRadius(), 5.14); QCOMPARE(sphere2->getParameters().size(), 14); sphere2->getParameter("Position")->setValueFromString("(1.11, 2.2, 3.9)"); Vector3DValue *position = dynamic_cast<Vector3DValue*>(sphere2->getParameter("Position")); QVERIFY(position != 0); QCOMPARE(position->getX(), 1.11); QCOMPARE(position->getY(), 2.2); QCOMPARE(position->getZ(), 3.9); SphereBody *copy = dynamic_cast<SphereBody*>(sphere2->createCopy()); QVERIFY(copy != 0); QCOMPARE(copy->getParameters().size(), 14); QVERIFY(copy->getCollisionObjects().size() == 1); QVERIFY(copy->getGeometries().size() == 1); Vector3DValue *copyPosition = dynamic_cast<Vector3DValue*>(copy->getParameter("Position")); QVERIFY(copyPosition != 0); QCOMPARE(copyPosition->getX(), 1.11); QCOMPARE(copyPosition->getY(), 2.2); QCOMPARE(copyPosition->getZ(), 3.9); SphereGeom *copyGeometry = dynamic_cast<SphereGeom*>(copy->getCollisionObjects().at(0)->getGeometry()); QVERIFY(copyGeometry != 0); QCOMPARE(copyGeometry->getRadius(), 5.14); CollisionObject *copyCollisionObject = copy->getCollisionObjects().at(0); QVERIFY(copyCollisionObject->getHostBody() == copy); QVERIFY(copyCollisionObject->getOwner() == 0); copy->setup(); QVERIFY(copyCollisionObject->getHostBody() == copy); QVERIFY(copyCollisionObject->getOwner() == copy); radius2->set(-10.0); sphere2->setup(); QVERIFY(sphereGeom2->getRadius() > 0.0); delete copy; delete sphere2; delete sphere; }