void particles::pit::myfunctions::CoordinatesRepresentationChange::beginIteration(particles::pit::State& solverState) {
l2_error_norm_histogram_ = new particles::pit::myfunctions::Histogram(
solverState, 1e-8, 1e-4);
max_error_norm_histogram_ = new particles::pit::myfunctions::Histogram(
solverState, 1e-7, 1e-4);
max_offset_norm_histogram_ = new particles::pit::myfunctions::Histogram(
solverState, 1e-3, 1e0);
_global_max_error = 0;
_globalMaxRelativeError = 0;
_globalMaxL2ErrorNorm = 0;
tarch::la::Vector<DIMENSIONS, double> zeroVector(0);
_globalL2ErrorNorm = zeroVector;
_globalNormAdditions = 0;
_maxRelativeErrorOut.str("");
_maxRelativeErrorOut.clear();
_maxErrorOut.str("");
_maxErrorOut.clear();
_maxOffsetOut.str("");
_maxOffsetOut.clear();
_RMSDOut.str("");
_RMSDOut.clear();
_L2ErrorNormOut.str("");
_L2ErrorNormOut.clear();
_L2NormOut.str("");
_L2NormOut.clear();
_MeanCoordinateOut.str("");
_MeanCoordinateOut.clear();
}
void simulateJumps::init()
{
//init the vector of waiting times.
_waitingTimeParams.clear();
_waitingTimeParams.resize(_alphabetSize);
int i, j;
for (i = 0; i < _alphabetSize; ++i)
{
_waitingTimeParams[i] = -_sp.dPij_dt(i, i, 0.0);
}
//init _jumpProbs.
//_jumpProbs[i][j] = Q[i][j] / -Q[i][i]
_jumpProbs.clear();
_jumpProbs.resize(_alphabetSize);
for (i = 0; i < _alphabetSize; ++i)
{
MDOUBLE sum = 0.0;
_jumpProbs[i].resize(_alphabetSize);
for (j = 0; j < _alphabetSize; ++j)
{
if (i == j)
_jumpProbs[i][j] = 0.0;
else
{
_jumpProbs[i][j] = _sp.dPij_dt(i, j, 0.0) / _waitingTimeParams[i];
}
sum += _jumpProbs[i][j];
}
if (! DEQUAL(sum, 1.0)){
string err = "error in simulateJumps::init(): sum probabilities is not 1 and equal to ";
err+=double2string(sum);
errorMsg::reportError(err);
}
}
//init _orderNodesVec: a vector in which the branch lengths are ordered in ascending order
_tree.getAllNodes(_orderNodesVec, _tree.getRoot());
sort(_orderNodesVec.begin(), _orderNodesVec.end(), simulateJumpsAbstract::compareDist);
_nodes2JumpsExp.clear();
_nodes2JumpsProb.clear();
VVdouble zeroMatrix(getCombinedAlphabetSize());
for (i = 0; i < getCombinedAlphabetSize(); ++i)
zeroMatrix[i].resize(getCombinedAlphabetSize(), 0.0);
Vdouble zeroVector(getCombinedAlphabetSize(),0.0);
for (i = 0; i < _orderNodesVec.size(); ++i)
{
string nodeName = _orderNodesVec[i]->name();
_nodes2JumpsExp[nodeName] = zeroMatrix;
_nodes2JumpsProb[nodeName] = zeroMatrix;
for (j=0; j<getCombinedAlphabetSize();++j)
_totalTerminals[nodeName]=zeroVector;
}
}
double
LogNormalJointPdf<V,M>::actualValue(
const V& domainVector,
const V* domainDirection,
V* gradVector,
M* hessianMatrix,
V* hessianEffect) const
{
if ((m_env.subDisplayFile()) && (m_env.displayVerbosity() >= 55)) {
*m_env.subDisplayFile() << "Entering LogNormalJointPdf<V,M>::actualValue()"
<< ", meanVector = " << *m_lawExpVector
<< ": domainVector = " << domainVector
<< ", domainVector.sizeLocal() = " << domainVector.sizeLocal()
<< ", this->m_domainSet.vectorSpace().dimLocal() = " << this->m_domainSet.vectorSpace().dimLocal()
<< std::endl;
}
UQ_FATAL_TEST_MACRO(domainVector.sizeLocal() != this->m_domainSet.vectorSpace().dimLocal(),
m_env.worldRank(),
"LogNormalJointPdf<V,M>::actualValue()",
"invalid input");
UQ_FATAL_TEST_MACRO((gradVector || hessianMatrix || hessianEffect),
m_env.worldRank(),
"LogNormalJointPdf<V,M>::actualValue()",
"incomplete code for gradVector, hessianMatrix and hessianEffect calculations");
double returnValue = 0.;
V zeroVector(domainVector);
zeroVector.cwSet(0.);
if (domainVector.atLeastOneComponentSmallerOrEqualThan(zeroVector)) {
returnValue = 0.;
}
else if (this->m_domainSet.contains(domainVector) == false) { // prudenci 2011-Oct-04
returnValue = 0.;
}
else {
returnValue = std::exp(this->lnValue(domainVector,domainDirection,gradVector,hessianMatrix,hessianEffect));
}
//returnValue *= exp(m_logOfNormalizationFactor); // No need, because 'lnValue()' is called right above // [PDF-10]
if ((m_env.subDisplayFile()) && (m_env.displayVerbosity() >= 55)) {
*m_env.subDisplayFile() << "Leaving LogNormalJointPdf<V,M>::actualValue()"
<< ", meanVector = " << *m_lawExpVector
<< ": domainVector = " << domainVector
<< ", returnValue = " << returnValue
<< std::endl;
}
return returnValue;
}
void CyclicCoordinateDescent::computeXBeta(void) {
// Note: X is current stored in (sparse) column-major format, which is
// inefficient for forming X\beta.
// TODO Make row-major version of X
if (setBetaList.empty()) { // Update all
// clear X\beta
zeroVector(hXBeta.data(), K);
for (int j = 0; j < J; ++j) {
axpyXBeta(hBeta[j], j);
}
} else {
while (!setBetaList.empty()) {
SetBetaEntry entry = setBetaList.front();
axpyXBeta(hBeta[entry.first] - entry.second, entry.first);
setBetaList.pop_front();
}
}
}
Vector VecArrayVectoraverage(const VecArray a)
{
int N, M, j;
Vector v;
g_assert(a);
N = VecArrayVectorSize(a);
if(N <= 0) return 0;
M = VecArraySize(a);
v = newVector(N);
zeroVector(v);
for(j = 0; j < M; j++) addtoVector(v, a[j]);
scaleVector(v, 1.0/M);
return v;
}
void GrBulletObject::resetPosition()
{
// http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=11000
rigidBody->clearForces();
btVector3 zeroVector(0, 0, 0);
rigidBody->setLinearVelocity(zeroVector);
rigidBody->setAngularVelocity(zeroVector);
btQuaternion rotation(btVector3((btScalar) startRotationAxis.x, (btScalar)startRotationAxis.y, (btScalar)startRotationAxis.z), (btScalar)(startRotationAngle * M_PI / 180));
btVector3 translate((btScalar) startOrigin.x, (btScalar) startOrigin.y, (btScalar) startOrigin.z);
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(translate);
startTransform.setRotation(rotation);
rigidBody->setWorldTransform(startTransform);
rigidBody->getMotionState()->setWorldTransform(startTransform);
}
void particles::pit::myfunctions::RepresentationChange::beginIteration(particles::pit::State& solverState) {
if(VERBOSE) {
std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!begin Iteration!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n";
}
particles::pit::myfunctions::CoordinatesRepresentationChange::beginIteration(solverState);
l2_error_norm_histogram_ = new particles::pit::myfunctions::Histogram(
solverState, 1e-7, 1e-3);
max_error_norm_histogram_ = new particles::pit::myfunctions::Histogram(
solverState, 1e-7, 1e-3);
max_offset_norm_histogram_ = new particles::pit::myfunctions::Histogram(
solverState, 1e-3, 1e0);
_global_max_error = 0;
_globalMaxOffset = 0;
_globalMaxRelativeError = 0;
_globalMaxL2ErrorNorm = 0;
tarch::la::Vector<DIMENSIONS, double> zeroVector(0);
_globalL2ErrorNorm = zeroVector;
_globalL2OffsetNorm = zeroVector;
_globalNormAdditions = 0;
_maxRelativeErrorOut.str("");
_maxRelativeErrorOut.clear();
_maxErrorOut.str("");
_maxErrorOut.clear();
_maxOffsetOut.str("");
_maxOffsetOut.clear();
_minOffsetOut.str("");
_minOffsetOut.clear();
_RMSDOut.str("");
_RMSDOut.clear();
_L2ErrorNormOut.str("");
_L2ErrorNormOut.clear();
_L2NormOut.str("");
_L2NormOut.clear();
_meanVelocityOut.str("");
_meanVelocityOut.clear();
}
// Given a point and a triangle (defined by three points), compute the closest point
// in the triangle. Clamp the point so it's confined to the area of the triangle.
vec3f NearestPointInTriangle(const vec3f &point, const vec3f &triangle0,
const vec3f &triangle1, const vec3f &triangle2)
{
vec3f zeroVector(0, 0, 0);
vec3f nearestPoint;
vec3f lineDelta0 = triangle1 - triangle0;
vec3f lineDelta1 = triangle2 - triangle0;
// Handle degenerate triangles
if ( (lineDelta0 == zeroVector) || (lineDelta1 == zeroVector) )
{
nearestPoint = NearestPointInLineSegment(point, triangle1, triangle2);
}
else if ( lineDelta0 == lineDelta1 )
{
nearestPoint = NearestPointInLineSegment(point, triangle0, triangle1);
}
else
{
static vec3f axis[3];
axis[0] = NearestPointInLine(triangle0, triangle1, triangle2);
axis[1] = NearestPointInLine(triangle1, triangle0, triangle2);
axis[2] = NearestPointInLine(triangle2, triangle0, triangle1);
float axisDot[3];
axisDot[0] = (triangle0-axis[0]).dot(point-axis[0]);
axisDot[1] = (triangle1-axis[1]).dot(point-axis[1]);
axisDot[2] = (triangle2-axis[2]).dot(point-axis[2]);
bool bForce = true;
float bestMagnitude2 = 0;
float closeMagnitude2;
vec3f closePoint;
if ( axisDot[0] < 0 )
{
closePoint = NearestPointInLineSegment(point, triangle1, triangle2);
closeMagnitude2 = point.dist2(closePoint);
if ( bForce || (bestMagnitude2 > closeMagnitude2) )
{
bForce = false;
bestMagnitude2 = closeMagnitude2;
nearestPoint = closePoint;
}
}
if ( axisDot[1] < 0 )
{
closePoint = NearestPointInLineSegment(point, triangle0, triangle2);
closeMagnitude2 = point.dist2(closePoint);
if ( bForce || (bestMagnitude2 > closeMagnitude2) )
{
bForce = false;
bestMagnitude2 = closeMagnitude2;
nearestPoint = closePoint;
}
}
if ( axisDot[2] < 0 )
{
closePoint = NearestPointInLineSegment(point, triangle0, triangle1);
closeMagnitude2 = point.dist2(closePoint);
if ( bForce || (bestMagnitude2 > closeMagnitude2) )
{
bForce = false;
bestMagnitude2 = closeMagnitude2;
nearestPoint = closePoint;
}
}
// If bForce is true at this point, it means the nearest point lies
// inside the triangle; use the nearest-point-on-a-plane equation
if ( bForce )
{
vec3f normal;
// Get the normal of the polygon (doesn't have to be a unit vector)
normal.cross(lineDelta0, lineDelta1);
vec3f pointDelta = point - triangle0;
float delta = normal.dot(pointDelta) / normal.dot(normal);
nearestPoint = point - delta*normal;
}
}
return nearestPoint;
}
double
LogNormalJointPdf<V,M>::lnValue(
const V& domainVector,
const V* domainDirection,
V* gradVector,
M* hessianMatrix,
V* hessianEffect) const
{
if ((m_env.subDisplayFile()) && (m_env.displayVerbosity() >= 55)) {
*m_env.subDisplayFile() << "Entering LogNormalJointPdf<V,M>::lnValue()"
<< ", meanVector = " << *m_lawExpVector
<< ": domainVector = " << domainVector
<< std::endl;
}
UQ_FATAL_TEST_MACRO((gradVector || hessianMatrix || hessianEffect),
m_env.worldRank(),
"LogNormalJointPdf<V,M>::lnValue()",
"incomplete code for gradVector, hessianMatrix and hessianEffect calculations");
if (domainDirection) {}; // just to remove compiler warning
double returnValue = 0.;
V zeroVector(domainVector);
zeroVector.cwSet(0.);
if (domainVector.atLeastOneComponentSmallerOrEqualThan(zeroVector)) {
returnValue = -INFINITY;
}
else if (this->m_domainSet.contains(domainVector) == false) { // prudenci 2011-Oct-04
returnValue = -INFINITY;
}
else {
if (m_diagonalCovMatrix) {
V diffVec(zeroVector);
for (unsigned int i = 0; i < domainVector.sizeLocal(); ++i) {
diffVec[i] = std::log(domainVector[i]) - this->lawExpVector()[i];
}
returnValue = ((diffVec*diffVec)/this->lawVarVector()).sumOfComponents();
returnValue *= -0.5;
if (m_normalizationStyle == 0) {
for (unsigned int i = 0; i < domainVector.sizeLocal(); ++i) {
returnValue -= std::log(domainVector[i] * std::sqrt(2. * M_PI * this->lawVarVector()[i])); // Contribution of 1/(x\sqrt{2\pi\sigma^2})
}
}
}
else {
UQ_FATAL_TEST_MACRO(true,
m_env.worldRank(),
"LogNormalJointPdf<V,M>::lnValue()",
"situation with a non-diagonal covariance matrix makes no sense");
}
returnValue += m_logOfNormalizationFactor; // [PDF-10]
}
if ((m_env.subDisplayFile()) && (m_env.displayVerbosity() >= 55)) {
*m_env.subDisplayFile() << "Leaving LogNormalJointPdf<V,M>::lnValue()"
<< ", meanVector = " << *m_lawExpVector
<< ": domainVector = " << domainVector
<< ", returnValue = " << returnValue
<< std::endl;
}
return returnValue;
}
void Physics::updateWorld(float dt){
dynamicsWorld->stepSimulation(dt, 10);
static btTransform transTable, transPaddle1, transPaddle2, transPuck;
static int timeTicks = 0;
if (playerScore->player1Score == 7) {
paused = true;
}
else if (playerScore->player2Score == 7) {
engine->play2D("../src/audio_files/media/youWin.wav");
paused = true;
}
if(oldPlayer2 == oldPlayer1 && playerScore->player2Score > playerScore->player1Score){
engine->play2D("../src/audio_files/media/gainedthelead.ogg");
}
else if(oldPlayer2 == oldPlayer1 && playerScore->player2Score < playerScore->player1Score){
engine->play2D("../src/audio_files/media/lost_the_lead.ogg");
}
if(oldPlayer1 < playerScore->player1Score)
{
objects[2].rigidBody->clearForces();
btVector3 zeroVector(0,0,0);
objects[2].rigidBody->setLinearVelocity(zeroVector);
objects[2].rigidBody->setAngularVelocity(zeroVector);
objects[2].rigidBody->translate(btVector3(0,-1,0) - transPuck.getOrigin());
oldPlayer1 = playerScore->player1Score;
}
else if(oldPlayer2 < playerScore->player2Score)
{
objects[2].rigidBody->clearForces();
btVector3 zeroVector(0,0,0);
objects[2].rigidBody->setLinearVelocity(zeroVector);
objects[2].rigidBody->setAngularVelocity(zeroVector);
objects[2].rigidBody->translate(btVector3(0,-1,0) - transPuck.getOrigin());
oldPlayer2 = playerScore->player2Score;
}
// paddle 1
objects[0].rigidBody->getMotionState()->getWorldTransform(transPaddle1);
btScalar mPaddle_1[16];
transPaddle1.getOpenGLMatrix(mPaddle_1);
objects[0].model.model = glm::make_mat4(mPaddle_1);
objects[0].model.model = glm::scale(objects[0].model.model, glm::vec3(2, 2, 2));
// table
objects[1].rigidBody->getMotionState()->getWorldTransform(transTable);
btScalar mTable[16];
transTable.getOpenGLMatrix(mTable);
objects[1].model.model = glm::make_mat4(mTable);
objects[1].model.model = glm::scale(objects[1].model.model, glm::vec3(5, 5, 5));
// puck
objects[2].rigidBody->getMotionState()->getWorldTransform(transPuck);
btScalar mPuck[16];
transPuck.getOpenGLMatrix(mPuck);
objects[2].model.model = glm::make_mat4(mPuck);
if (!twoPlayer)
{
timeTicks++;
// collision just occurred
if (*follow == 0)
{
timeTicks = 0;
if (transPaddle1.getOrigin().z() > -11.0f)
objects[0].rigidBody->translate((transPaddle1.getOrigin() + btVector3(0,0,-1)) - transPaddle1.getOrigin());
*follow = 1;
}
if (timeTicks >= 50)
*follow = 1;
if (transPuck.getOrigin().z() > 0)
{
objects[0].rigidBody->translate(btVector3(0,-1,-7) - transPaddle1.getOrigin());
*follow = 1;
}
else if (((objects[2].rigidBody->getLinearVelocity().x() != 0) || (objects[2].rigidBody->getLinearVelocity().z() != 0)) && *follow)
{
float time = getInterceptTime(transPuck);
// compute position of puck at time of intercept
btVector3 interceptPos = transPuck.getOrigin() + objects[2].rigidBody->getLinearVelocity() * time;
// move paddle to intercept the puck
btVector3 direction = interceptPos - transPaddle1.getOrigin();
direction.normalize();
float speed = 10.0f;
objects[0].rigidBody->setLinearVelocity(btVector3( speed * direction.x(), 0.0, speed * direction.z()));
}
// check for collision between AI paddle and puck
dynamicsWorld->contactPairTest(objects[0].rigidBody, objects[2].rigidBody, *AICollisionBack);
}
// paddle 2
objects[3].rigidBody->getMotionState()->getWorldTransform(transPaddle2);
btScalar mPaddle_2[16];
transPaddle2.getOpenGLMatrix(mPaddle_2);
objects[3].model.model = glm::make_mat4(mPaddle_2);
objects[3].model.model = glm::scale(objects[3].model.model, glm::vec3(2, 2, 2));
// check for collision with left goal
dynamicsWorld->contactPairTest(objects[5].rigidBody, objects[2].rigidBody, *goalCollisionCallBack);
// check for collision with right goal
dynamicsWorld->contactPairTest(objects[2].rigidBody, objects[6].rigidBody, *goalCollisionCallBack);
}
