const Vector & SSPquadUP::getResistingForceIncInertia() { // terms stemming from acceleration const Vector &accel1 = theNodes[0]->getTrialAccel(); const Vector &accel2 = theNodes[1]->getTrialAccel(); const Vector &accel3 = theNodes[2]->getTrialAccel(); const Vector &accel4 = theNodes[3]->getTrialAccel(); // compute current resisting force this->getResistingForce(); // compute mass matrix this->getMass(); Vector a(12); a(0) = accel1(0); a(1) = accel1(1); a(2) = accel1(2); a(3) = accel2(0); a(4) = accel2(1); a(5) = accel2(2); a(6) = accel3(0); a(7) = accel3(1); a(8) = accel3(2); a(9) = accel4(0); a(10) = accel4(1); a(11) = accel4(2); mInternalForces.addMatrixVector(1.0, mMass, a, 1.0); // terms stemming from velocity const Vector &vel1 = theNodes[0]->getTrialVel(); const Vector &vel2 = theNodes[1]->getTrialVel(); const Vector &vel3 = theNodes[2]->getTrialVel(); const Vector &vel4 = theNodes[3]->getTrialVel(); Vector v(12); v(0) = vel1(0); v(1) = vel1(1); v(2) = vel1(2); v(3) = vel2(0); v(4) = vel2(1); v(5) = vel2(2); v(6) = vel3(0); v(7) = vel3(1); v(8) = vel3(2); v(9) = vel4(0); v(10) = vel4(1); v(11) = vel4(2); // compute damping matrix this->getDamp(); mInternalForces.addMatrixVector(1.0, mDamp, v, 1.0); return mInternalForces; }
/*! Returns the correct coefficient of friction for this contact. If either body is dynamic, and the relative velocity between them is greater than 1.0 mm/sec (should be made a parameter), then it returns the kinetic COF, otherwise it returns the static COF. */ double Contact::getCof() const { DynamicBody *db; vec3 radius,vel1(vec3::ZERO),vel2(vec3::ZERO),rotvel; if (body1->isDynamic()) { db = (DynamicBody *)body1; radius = db->getTran().rotation() * (loc - db->getCoG()); vel1.set(db->getVelocity()[0],db->getVelocity()[1],db->getVelocity()[2]); rotvel.set(db->getVelocity()[3],db->getVelocity()[4],db->getVelocity()[5]); vel1 += radius * rotvel; } if (body2->isDynamic()) { db = (DynamicBody *)body2; radius = db->getTran().rotation() * (mate->loc - db->getCoG()); vel2.set(db->getVelocity()[0],db->getVelocity()[1],db->getVelocity()[2]); rotvel.set(db->getVelocity()[3],db->getVelocity()[4],db->getVelocity()[5]); vel2 += radius * rotvel; } if ((vel1 - vel2).len() > 1.0) { DBGP("SLIDING!"); return kcof; } else return cof; }
int TwoNodeLink::update() { int errCode = 0; // get global trial response const Vector &dsp1 = theNodes[0]->getTrialDisp(); const Vector &dsp2 = theNodes[1]->getTrialDisp(); const Vector &vel1 = theNodes[0]->getTrialVel(); const Vector &vel2 = theNodes[1]->getTrialVel(); int numDOF2 = numDOF/2; Vector ug(numDOF), ugdot(numDOF), uldot(numDOF); for (int i=0; i<numDOF2; i++) { ug(i) = dsp1(i); ugdot(i) = vel1(i); ug(i+numDOF2) = dsp2(i); ugdot(i+numDOF2) = vel2(i); } // transform response from the global to the local system ul.addMatrixVector(0.0, Tgl, ug, 1.0); uldot.addMatrixVector(0.0, Tgl, ugdot, 1.0); // transform response from the local to the basic system ub.addMatrixVector(0.0, Tlb, ul, 1.0); ubdot.addMatrixVector(0.0, Tlb, uldot, 1.0); //ub = (Tlb*Tgl)*ug; //ubdot = (Tlb*Tgl)*ugdot; // set trial response for material models for (int i=0; i<numDir; i++) errCode += theMaterials[i]->setTrialStrain(ub(i),ubdot(i)); return errCode; }
CollisionAttributes Hurtbox::resolveHurtboxCollision(Hurtbox* hb, MapObject* obj1, MapObject* obj2) { CollisionAttributes c(obj1); // Very simple bouncing simulation, utilizing the property of total elastic collision // The resulting angles are not entirely correct at this point double mass1 = 1, mass2 = 1; // Mass should be determined in each MapObject later Vector2D vel1(obj1->getXVel(), obj1->getYVel()); Vector2D vel2(obj2->getXVel(), obj2->getYVel()); // Calculate the new velocities Vector2D newVel = (vel1 * (mass1 - mass2) + vel2 * 2 * mass2) / (mass1 + mass2); Vector2D* finalVel = new Vector2D(newVel.getX(), newVel.getY()); c.setVelocity(finalVel); return c; }
double Truss::computeCurrentStrainRate(void) const { // NOTE method will not be called if L == 0 // determine the strain const Vector &vel1 = theNodes[0]->getTrialVel(); const Vector &vel2 = theNodes[1]->getTrialVel(); double dLength = 0.0; for (int i = 0; i < dimension; i++) dLength += (vel2(i)-vel1(i))*cosX[i]; // this method should never be called with L == 0 return dLength/L; }
int EETruss::update() { int rValue = 0; // get current time Domain *theDomain = this->getDomain(); (*t)(0) = theDomain->getCurrentTime(); // determine dsp, vel and acc in basic system const Vector &disp1 = theNodes[0]->getTrialDisp(); const Vector &disp2 = theNodes[1]->getTrialDisp(); const Vector &vel1 = theNodes[0]->getTrialVel(); const Vector &vel2 = theNodes[1]->getTrialVel(); const Vector &accel1 = theNodes[0]->getTrialAccel(); const Vector &accel2 = theNodes[1]->getTrialAccel(); const Vector &dispIncr1 = theNodes[0]->getIncrDeltaDisp(); const Vector &dispIncr2 = theNodes[1]->getIncrDeltaDisp(); (*db)(0) = (*vb)(0) = (*ab)(0) = 0.0; double dbDelta = 0.0; for (int i=0; i<numDIM; i++) { (*db)(0) += (disp2(i)-disp1(i))*cosX[i]; (*vb)(0) += (vel2(i)-vel1(i))*cosX[i]; (*ab)(0) += (accel2(i)-accel1(i))*cosX[i]; dbDelta += (dispIncr2(i)-dispIncr1(i))*cosX[i]; } // do not check time for right now because of transformation constraint // handler calling update at beginning of new step when applying load // if (fabs(dbDelta) > DBL_EPSILON || (*t)(0) > tLast) { if (fabs(dbDelta) > DBL_EPSILON) { // set the trial response at the site if (theSite != 0) { theSite->setTrialResponse(db, vb, ab, (Vector*)0, t); } else { sData[0] = OF_RemoteTest_setTrialResponse; rValue += theChannel->sendVector(0, 0, *sendData, 0); } } // save the last time tLast = (*t)(0); return rValue; }
const Vector & PDeltaCrdTransf2d::getBasicTrialVel(void) { // determine global velocities const Vector &vel1 = nodeIPtr->getTrialVel(); const Vector &vel2 = nodeJPtr->getTrialVel(); static double vg[6]; for (int i = 0; i < 3; i++) { vg[i] = vel1(i); vg[i+3] = vel2(i); } static Vector vb(3); double oneOverL = 1.0/L; double sl = sinTheta*oneOverL; double cl = cosTheta*oneOverL; vb(0) = -cosTheta*vg[0] - sinTheta*vg[1] + cosTheta*vg[3] + sinTheta*vg[4]; vb(1) = -sl*vg[0] + cl*vg[1] + vg[2] + sl*vg[3] - cl*vg[4]; if (nodeIOffset != 0) { double t02 = -cosTheta*nodeIOffset[1] + sinTheta*nodeIOffset[0]; double t12 = sinTheta*nodeIOffset[1] + cosTheta*nodeIOffset[0]; vb(0) -= t02*vg[2]; vb(1) += oneOverL*t12*vg[2]; } if (nodeJOffset != 0) { double t35 = -cosTheta*nodeJOffset[1] + sinTheta*nodeJOffset[0]; double t45 = sinTheta*nodeJOffset[1] + cosTheta*nodeJOffset[0]; vb(0) += t35*vg[5]; vb(1) -= oneOverL*t45*vg[5]; } vb(2) = vb(1) + vg[5] - vg[2]; return vb; }
int EETrussCorot::update() { int rValue = 0; // get current time Domain *theDomain = this->getDomain(); (*t)(0) = theDomain->getCurrentTime(); // determine dsp, vel and acc in basic system const Vector &disp1 = theNodes[0]->getTrialDisp(); const Vector &disp2 = theNodes[1]->getTrialDisp(); const Vector &vel1 = theNodes[0]->getTrialVel(); const Vector &vel2 = theNodes[1]->getTrialVel(); const Vector &accel1 = theNodes[0]->getTrialAccel(); const Vector &accel2 = theNodes[1]->getTrialAccel(); const Vector &dispIncr1 = theNodes[0]->getIncrDeltaDisp(); const Vector &dispIncr2 = theNodes[1]->getIncrDeltaDisp(); // initial offsets double d21Last[3]; d21[0] = L; d21[1] = d21[2] = 0.0; v21[0] = v21[1] = v21[2] = 0.0; a21[0] = a21[1] = a21[2] = 0.0; d21Last[0] = L; d21Last[1] = d21Last[2] = 0.0; // update offsets in basic system for (int i=0; i<numDIM; i++) { double deltaDisp = disp2(i) - disp1(i); d21[0] += deltaDisp*R(0,i); d21[1] += deltaDisp*R(1,i); d21[2] += deltaDisp*R(2,i); double deltaVel = vel2(i) - vel1(i); v21[0] += deltaVel*R(0,i); v21[1] += deltaVel*R(1,i); v21[2] += deltaVel*R(2,i); double deltaAccel = accel2(i) - accel1(i); a21[0] += deltaAccel*R(0,i); a21[1] += deltaAccel*R(1,i); a21[2] += deltaAccel*R(2,i); double deltaDispLast = (disp2(i)-dispIncr2(i)) - (disp1(i)-dispIncr1(i)); d21Last[0] += deltaDispLast*R(0,i); d21Last[1] += deltaDispLast*R(1,i); d21Last[2] += deltaDispLast*R(2,i); } // compute new length and deformation Ln = sqrt(d21[0]*d21[0] + d21[1]*d21[1] + d21[2]*d21[2]); double c1 = d21[0]*v21[0] + d21[1]*v21[1] + d21[2]*v21[2]; double c2 = v21[0]*v21[0] + v21[1]*v21[1] + v21[2]*v21[2] + d21[0]*a21[0] + d21[1]*a21[1] + d21[2]*a21[2]; (*db)(0) = Ln - L; (*vb)(0) = c1/Ln; (*ab)(0) = c2/Ln - (c1*c1)/(Ln*Ln*Ln); double LnLast = sqrt(d21Last[0]*d21Last[0] + d21Last[1]*d21Last[1] + d21Last[2]*d21Last[2]); double dbDelta = (Ln - L) - (LnLast - L); // do not check time for right now because of transformation constraint // handler calling update at beginning of new step when applying load // if (fabs(dbDelta) > DBL_EPSILON || (*t)(0) > tLast) { if (fabs(dbDelta) > DBL_EPSILON) { // set the trial response at the site if (theSite != 0) { theSite->setTrialResponse(db, vb, ab, (Vector*)0, t); } else { sData[0] = OF_RemoteTest_setTrialResponse; rValue += theChannel->sendVector(0, 0, *sendData, 0); } } // save the last time tLast = (*t)(0); return rValue; }
int ElastomericBearingBoucWen2d::update() { // get global trial displacements and velocities const Vector &dsp1 = theNodes[0]->getTrialDisp(); const Vector &dsp2 = theNodes[1]->getTrialDisp(); const Vector &vel1 = theNodes[0]->getTrialVel(); const Vector &vel2 = theNodes[1]->getTrialVel(); static Vector ug(6), ugdot(6), uldot(6), ubdot(3); for (int i=0; i<3; i++) { ug(i) = dsp1(i); ugdot(i) = vel1(i); ug(i+3) = dsp2(i); ugdot(i+3) = vel2(i); } // transform response from the global to the local system ul.addMatrixVector(0.0, Tgl, ug, 1.0); uldot.addMatrixVector(0.0, Tgl, ugdot, 1.0); // transform response from the local to the basic system ub.addMatrixVector(0.0, Tlb, ul, 1.0); ubdot.addMatrixVector(0.0, Tlb, uldot, 1.0); // 1) get axial force and stiffness in basic x-direction theMaterials[0]->setTrialStrain(ub(0), ubdot(0)); qb(0) = theMaterials[0]->getStress(); kb(0,0) = theMaterials[0]->getTangent(); // 2) calculate shear force and stiffness in basic y-direction // get displacement increment (trial - commited) double delta_ub = ub(1) - ubC(1); if (fabs(delta_ub) > 0.0) { // get yield displacement double uy = qYield/k0; // calculate hysteretic evolution parameter z using Newton-Raphson int iter = 0; double zAbs, tmp1, f, Df, delta_z; do { zAbs = fabs(z); if (zAbs == 0.0) // check because of negative exponents zAbs = DBL_EPSILON; tmp1 = gamma + beta*sgn(z*delta_ub); // function and derivative f = z - zC - delta_ub/uy*(A - pow(zAbs,eta)*tmp1); Df = 1.0 + delta_ub/uy*eta*pow(zAbs,eta-1.0)*sgn(z)*tmp1; // issue warning if derivative Df is zero if (fabs(Df) <= DBL_EPSILON) { opserr << "WARNING: ElastomericBearingBoucWen2d::update() - " << "zero derivative in Newton-Raphson scheme for " << "hysteretic evolution parameter z.\n"; return -1; } // advance one step delta_z = f/Df; z -= delta_z; iter++; } while ((fabs(delta_z) >= tol) && (iter < maxIter)); // issue warning if Newton-Raphson scheme did not converge if (iter >= maxIter) { opserr << "WARNING: ElastomericBearingBoucWen2d::update() - " << "did not find the hysteretic evolution parameter z after " << iter << " iterations and norm: " << fabs(delta_z) << endln; return -2; } // get derivative of hysteretic evolution parameter * uy dzdu = A - pow(fabs(z),eta)*(gamma + beta*sgn(z*delta_ub)); // set shear force qb(1) = qYield*z + k2*ub(1) + k3*sgn(ub(1))*pow(fabs(ub(1)),mu); // set tangent stiffness kb(1,1) = k0*dzdu + k2 + k3*mu*pow(fabs(ub(1)),mu-1.0); } // 3) get moment and stiffness about basic z-direction theMaterials[1]->setTrialStrain(ub(2), ubdot(2)); qb(2) = theMaterials[1]->getStress(); kb(2,2) = theMaterials[1]->getTangent(); return 0; }
int SingleFPSimple2d::update() { // get global trial displacements and velocities const Vector &dsp1 = theNodes[0]->getTrialDisp(); const Vector &dsp2 = theNodes[1]->getTrialDisp(); const Vector &vel1 = theNodes[0]->getTrialVel(); const Vector &vel2 = theNodes[1]->getTrialVel(); static Vector ug(6), ugdot(6), uldot(6), ubdot(3); for (int i=0; i<3; i++) { ug(i) = dsp1(i); ugdot(i) = vel1(i); ug(i+3) = dsp2(i); ugdot(i+3) = vel2(i); } // transform response from the global to the local system ul = Tgl*ug; uldot = Tgl*ugdot; // transform response from the local to the basic system ub = Tlb*ul; ubdot = Tlb*uldot; // get absolute velocity double ubdotAbs = sqrt(pow(ubdot(1)/Reff*ub(1),2) + pow(ubdot(1),2)); // 1) get axial force and stiffness in basic x-direction double ub0Old = theMaterials[0]->getStrain(); theMaterials[0]->setTrialStrain(ub(0),ubdot(0)); qb(0) = theMaterials[0]->getStress(); kb(0,0) = theMaterials[0]->getTangent(); // check for uplift if (qb(0) >= 0.0) { kb = kbInit; if (qb(0) > 0.0) { theMaterials[0]->setTrialStrain(ub0Old,0.0); kb(0,0) *= DBL_EPSILON; kb(2,2) *= DBL_EPSILON; // update plastic displacement ubPlastic = ub(1); } qb.Zero(); return 0; } // 2) calculate shear force and stiffness in basic y-direction int iter = 0; double qb1Old = 0.0; do { // save old shear force qb1Old = qb(1); // get normal and friction (yield) forces double N = -qb(0) + qb(1)/Reff*ub(1) - qb(1)*ul(2); theFrnMdl->setTrial(N, ubdotAbs); double qYield = (theFrnMdl->getFrictionForce()); // get stiffness of elastic component double k2 = N/Reff; // get initial stiffness of hysteretic component double k0 = qYield/uy; // get trial shear force of hysteretic component double qTrial = k0*(ub(1) - ubPlasticC); // compute yield criterion of hysteretic component double qTrialNorm = fabs(qTrial); double Y = qTrialNorm - qYield; // elastic step -> no updates required if (Y <= 0.0) { // set shear force qb(1) = qTrial + k2*ub(1) - N*ul(2); // set tangent stiffness kb(1,1) = k0 + k2; } // plastic step -> return mapping else { // compute consistency parameter double dGamma = Y/k0; // update plastic displacement ubPlastic = ubPlasticC + dGamma*qTrial/qTrialNorm; // set shear force qb(1) = qYield*qTrial/qTrialNorm + k2*ub(1) - N*ul(2); // set tangent stiffness kb(1,1) = k2; } iter++; } while ((fabs(qb(1)-qb1Old) >= tol) && (iter < maxIter)); // issue warning if iteration did not converge if (iter >= maxIter) { opserr << "WARNING: SingleFPSimple2d::update() - did not find the shear force after " << iter << " iterations and norm: " << fabs(qb(1)-qb1Old) << endln; return -1; } // 3) get moment and stiffness in basic z-direction theMaterials[1]->setTrialStrain(ub(2),ubdot(2)); qb(2) = theMaterials[1]->getStress(); kb(2,2) = theMaterials[1]->getTangent(); return 0; }
void dgWorldDynamicUpdate::ResolveClusterForces(dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const { if (cluster->m_activeJointCount) { SortClusters(cluster, timestep, threadID); } if (!cluster->m_isContinueCollision) { if (cluster->m_activeJointCount) { BuildJacobianMatrix (cluster, threadID, timestep); CalculateClusterReactionForces(cluster, threadID, timestep, DG_SOLVER_MAX_ERROR); //CalculateClusterReactionForces_1(cluster, threadID, timestep, DG_SOLVER_MAX_ERROR); } else { IntegrateExternalForce(cluster, timestep, threadID); } IntegrateVelocity (cluster, DG_SOLVER_MAX_ERROR, timestep, threadID); } else { // calculate reaction forces and new velocities BuildJacobianMatrix (cluster, threadID, timestep); IntegrateReactionsForces (cluster, threadID, timestep, DG_SOLVER_MAX_ERROR); // see if the island goes to sleep bool isAutoSleep = true; bool stackSleeping = true; dgInt32 sleepCounter = 10000; dgWorld* const world = (dgWorld*) this; const dgInt32 bodyCount = cluster->m_bodyCount; dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; dgBodyInfo* const bodyArray = &bodyArrayPtr[cluster->m_bodyStart]; const dgFloat32 forceDamp = DG_FREEZZING_VELOCITY_DRAG; dgFloat32 maxAccel = dgFloat32 (0.0f); dgFloat32 maxAlpha = dgFloat32 (0.0f); dgFloat32 maxSpeed = dgFloat32 (0.0f); dgFloat32 maxOmega = dgFloat32 (0.0f); const dgFloat32 speedFreeze = world->m_freezeSpeed2; const dgFloat32 accelFreeze = world->m_freezeAccel2; const dgVector forceDampVect (forceDamp, forceDamp, forceDamp, dgFloat32 (0.0f)); for (dgInt32 i = 1; i < bodyCount; i ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { dgAssert (body->m_invMass.m_w); const dgFloat32 accel2 = body->m_accel.DotProduct3(body->m_accel); const dgFloat32 alpha2 = body->m_alpha.DotProduct3(body->m_alpha); const dgFloat32 speed2 = body->m_veloc.DotProduct3(body->m_veloc); const dgFloat32 omega2 = body->m_omega.DotProduct3(body->m_omega); maxAccel = dgMax (maxAccel, accel2); maxAlpha = dgMax (maxAlpha, alpha2); maxSpeed = dgMax (maxSpeed, speed2); maxOmega = dgMax (maxOmega, omega2); bool equilibrium = (accel2 < accelFreeze) && (alpha2 < accelFreeze) && (speed2 < speedFreeze) && (omega2 < speedFreeze); if (equilibrium) { dgVector veloc (body->m_veloc * forceDampVect); dgVector omega = body->m_omega * forceDampVect; body->m_veloc = (dgVector (veloc.DotProduct4(veloc)) > m_velocTol) & veloc; body->m_omega = (dgVector (omega.DotProduct4(omega)) > m_velocTol) & omega; } body->m_equilibrium = dgUnsigned32 (equilibrium); stackSleeping &= equilibrium; isAutoSleep &= body->m_autoSleep; sleepCounter = dgMin (sleepCounter, body->m_sleepingCounter); } // clear accel and angular acceleration body->m_accel = dgVector::m_zero; body->m_alpha = dgVector::m_zero; } if (isAutoSleep) { if (stackSleeping) { // the island went to sleep mode, for (dgInt32 i = 1; i < bodyCount; i ++) { dgBody* const body = bodyArray[i].m_body; dgAssert (body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || body->IsRTTIType (dgBody::m_kinematicBodyRTTI)); body->m_accel = dgVector::m_zero; body->m_alpha = dgVector::m_zero; body->m_veloc = dgVector::m_zero; body->m_omega = dgVector::m_zero; } } else { // island is not sleeping but may be resting with small residual velocity for a long time // see if we can force to go to sleep if ((maxAccel > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel) || (maxAlpha > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha) || (maxSpeed > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc) || (maxOmega > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega)) { for (dgInt32 i = 1; i < bodyCount; i ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->m_sleepingCounter = 0; } } } else { dgInt32 index = 0; for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) { if ((maxAccel <= world->m_sleepTable[i].m_maxAccel) && (maxAlpha <= world->m_sleepTable[i].m_maxAlpha) && (maxSpeed <= world->m_sleepTable[i].m_maxVeloc) && (maxOmega <= world->m_sleepTable[i].m_maxOmega)) { index = i; break; } } dgInt32 timeScaleSleepCount = dgInt32 (dgFloat32 (60.0f) * sleepCounter * timestep); if (timeScaleSleepCount > world->m_sleepTable[index].m_steps) { // force island to sleep stackSleeping = true; for (dgInt32 i = 1; i < bodyCount; i ++) { dgBody* const body = bodyArray[i].m_body; dgAssert (body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || body->IsRTTIType (dgBody::m_kinematicBodyRTTI)); body->m_accel = dgVector::m_zero; body->m_alpha = dgVector::m_zero; body->m_veloc = dgVector::m_zero; body->m_omega = dgVector::m_zero; body->m_equilibrium = true; } } else { sleepCounter ++; for (dgInt32 i = 1; i < bodyCount; i ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->m_sleepingCounter = sleepCounter; } } } } } } if (!(isAutoSleep & stackSleeping)) { // island is not sleeping, need to integrate island velocity const dgUnsigned32 lru = world->GetBroadPhase()->m_lru; const dgInt32 jointCount = cluster->m_jointCount; dgJointInfo* const constraintArrayPtr = (dgJointInfo*) &world->m_jointsMemory[0]; dgJointInfo* const constraintArray = &constraintArrayPtr[cluster->m_jointStart]; dgFloat32 timeRemaining = timestep; const dgFloat32 timeTol = dgFloat32 (0.01f) * timestep; for (dgInt32 i = 0; (i < DG_MAX_CONTINUE_COLLISON_STEPS) && (timeRemaining > timeTol); i ++) { // calculate the closest time to impact dgFloat32 timeToImpact = timeRemaining; for (dgInt32 j = 0; (j < jointCount) && (timeToImpact > timeTol); j ++) { dgContact* const contact = (dgContact*) constraintArray[j].m_joint; if (contact->GetId() == dgConstraint::m_contactConstraint) { dgDynamicBody* const body0 = (dgDynamicBody*)contact->m_body0; dgDynamicBody* const body1 = (dgDynamicBody*)contact->m_body1; if (body0->m_continueCollisionMode | body1->m_continueCollisionMode) { dgVector p; dgVector q; dgVector normal; timeToImpact = dgMin (timeToImpact, world->CalculateTimeToImpact (contact, timeToImpact, threadID, p, q, normal, dgFloat32 (-1.0f / 256.0f))); } } } if (timeToImpact > timeTol) { timeRemaining -= timeToImpact; for (dgInt32 j = 1; j < bodyCount; j ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->IntegrateVelocity(timeToImpact); body->UpdateWorlCollisionMatrix(); } } } else { if (timeToImpact >= dgFloat32 (-1.0e-5f)) { for (dgInt32 j = 1; j < bodyCount; j++) { dgDynamicBody* const body = (dgDynamicBody*)bodyArray[j].m_body; if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { body->IntegrateVelocity(timeToImpact); body->UpdateWorlCollisionMatrix(); } } } CalculateClusterContacts (cluster, timeRemaining, lru, threadID); BuildJacobianMatrix (cluster, threadID, 0.0f); IntegrateReactionsForces (cluster, threadID, 0.0f, DG_SOLVER_MAX_ERROR); bool clusterReceding = true; const dgFloat32 step = timestep * dgFloat32 (1.0f / DG_MAX_CONTINUE_COLLISON_STEPS); for (dgInt32 k = 0; (k < DG_MAX_CONTINUE_COLLISON_STEPS) && clusterReceding; k ++) { dgFloat32 smallTimeStep = dgMin (step, timeRemaining); timeRemaining -= smallTimeStep; for (dgInt32 j = 1; j < bodyCount; j ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->IntegrateVelocity (smallTimeStep); body->UpdateWorlCollisionMatrix(); } } clusterReceding = false; if (timeRemaining > timeTol) { CalculateClusterContacts (cluster, timeRemaining, lru, threadID); bool isColliding = false; for (dgInt32 j = 0; (j < jointCount) && !isColliding; j ++) { dgContact* const contact = (dgContact*) constraintArray[j].m_joint; if (contact->GetId() == dgConstraint::m_contactConstraint) { const dgBody* const body0 = contact->m_body0; const dgBody* const body1 = contact->m_body1; const dgVector& veloc0 = body0->m_veloc; const dgVector& veloc1 = body1->m_veloc; const dgVector& omega0 = body0->m_omega; const dgVector& omega1 = body1->m_omega; const dgVector& com0 = body0->m_globalCentreOfMass; const dgVector& com1 = body1->m_globalCentreOfMass; for (dgList<dgContactMaterial>::dgListNode* node = contact->GetFirst(); node; node = node->GetNext()) { const dgContactMaterial* const contactMaterial = &node->GetInfo(); dgVector vel0 (veloc0 + omega0.CrossProduct3(contactMaterial->m_point - com0)); dgVector vel1 (veloc1 + omega1.CrossProduct3(contactMaterial->m_point - com1)); dgVector vRel (vel0 - vel1); dgAssert (contactMaterial->m_normal.m_w == dgFloat32 (0.0f)); dgFloat32 speed = vRel.DotProduct4(contactMaterial->m_normal).m_w; isColliding |= (speed < dgFloat32 (0.0f)); } } } clusterReceding = !isColliding; } } } } if (timeRemaining > dgFloat32 (0.0)) { for (dgInt32 j = 1; j < bodyCount; j ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->IntegrateVelocity(timeRemaining); body->UpdateCollisionMatrix (timeRemaining, threadID); } } } else { for (dgInt32 j = 1; j < bodyCount; j ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->UpdateCollisionMatrix (timestep, threadID); } } } } } }
const Vector& FourNodeQuadUP::getResistingForceIncInertia() { int i, j, k; const Vector &accel1 = nd1Ptr->getTrialAccel(); const Vector &accel2 = nd2Ptr->getTrialAccel(); const Vector &accel3 = nd3Ptr->getTrialAccel(); const Vector &accel4 = nd4Ptr->getTrialAccel(); static double a[12]; a[0] = accel1(0); a[1] = accel1(1); a[2] = accel1(2); a[3] = accel2(0); a[4] = accel2(1); a[5] = accel2(2); a[6] = accel3(0); a[7] = accel3(1); a[8] = accel3(2); a[9] = accel4(0); a[10] = accel4(1); a[11] = accel4(2); // Compute the current resisting force this->getResistingForce(); //opserr<<"K "<<P<<endln; // Compute the mass matrix this->getMass(); for (i = 0; i < 12; i++) { for (j = 0; j < 12; j++) P(i) += K(i,j)*a[j]; } //opserr<<"K+M "<<P<<endln; // dynamic seepage force /*for (i = 0, k = 0; i < 4; i++, k += 3) { // loop over integration points for (j = 0; j < 4; j++) { P(i+2) -= rho*dvol[j]*(shp[2][i][j]*a[k]*perm[0]*shp[0][i][j] +shp[2][i][j]*a[k+1]*perm[1]*shp[1][i][j]); } }*/ //opserr<<"K+M+fb "<<P<<endln; const Vector &vel1 = nd1Ptr->getTrialVel(); const Vector &vel2 = nd2Ptr->getTrialVel(); const Vector &vel3 = nd3Ptr->getTrialVel(); const Vector &vel4 = nd4Ptr->getTrialVel(); a[0] = vel1(0); a[1] = vel1(1); a[2] = vel1(2); a[3] = vel2(0); a[4] = vel2(1); a[5] = vel2(2); a[6] = vel3(0); a[7] = vel3(1); a[8] = vel3(2); a[9] = vel4(0); a[10] = vel4(1); a[11] = vel4(2); this->getDamp(); for (i = 0; i < 12; i++) { for (j = 0; j < 12; j++) { P(i) += K(i,j)*a[j]; } } //opserr<<"final "<<P<<endln; return P; }
int ElastomericBearing2d::update() { // get global trial displacements and velocities const Vector &dsp1 = theNodes[0]->getTrialDisp(); const Vector &dsp2 = theNodes[1]->getTrialDisp(); const Vector &vel1 = theNodes[0]->getTrialVel(); const Vector &vel2 = theNodes[1]->getTrialVel(); static Vector ug(6), ugdot(6), uldot(6), ubdot(3); for (int i=0; i<3; i++) { ug(i) = dsp1(i); ugdot(i) = vel1(i); ug(i+3) = dsp2(i); ugdot(i+3) = vel2(i); } // transform response from the global to the local system ul = Tgl*ug; uldot = Tgl*ugdot; // transform response from the local to the basic system ub = Tlb*ul; ubdot = Tlb*uldot; // 1) get axial force and stiffness in basic x-direction theMaterials[0]->setTrialStrain(ub(0),ubdot(0)); qb(0) = theMaterials[0]->getStress(); kb(0,0) = theMaterials[0]->getTangent(); // 2) calculate shear force and stiffness in basic y-direction // get trial shear force of hysteretic component double qTrial = k0*(ub(1) - ubPlasticC); // compute yield criterion of hysteretic component double qTrialNorm = fabs(qTrial); double Y = qTrialNorm - qYield; // elastic step -> no updates required if (Y <= 0.0) { // set shear force qb(1) = qTrial + k2*ub(1); // set tangent stiffness kb(1,1) = k0 + k2; } // plastic step -> return mapping else { // compute consistency parameter double dGamma = Y/k0; // update plastic displacement ubPlastic = ubPlasticC + dGamma*qTrial/qTrialNorm; // set shear force qb(1) = qYield*qTrial/qTrialNorm + k2*ub(1); // set tangent stiffness kb(1,1) = k2; } // 3) get moment and stiffness in basic z-direction theMaterials[1]->setTrialStrain(ub(2),ubdot(2)); qb(2) = theMaterials[1]->getStress(); kb(2,2) = theMaterials[1]->getTangent(); return 0; }
int main() { std::mt19937 generator(time(nullptr)); sys::ComputeSystem cs; cs.create(sys::ComputeSystem::_gpu); sys::ComputeProgram prog; prog.loadFromFile("resources/neoKernels.cl", cs); // --------------------------- Create the Sparse Coder --------------------------- cl::Image2D inputImage = cl::Image2D(cs.getContext(), CL_MEM_READ_WRITE, cl::ImageFormat(CL_R, CL_FLOAT), 64, 64); std::ifstream fromFile("resources/train-images.idx3-ubyte", std::ios::binary | std::ios::in); if (!fromFile.is_open()) { std::cerr << "Could not open train-images.idx3-ubyte!" << std::endl; return 1; } std::vector<neo::PredictiveHierarchy::LayerDesc> layerDescs(4); layerDescs[0]._size = { 64, 64 }; layerDescs[0]._feedForwardRadius = 8; layerDescs[1]._size = { 48, 48 }; layerDescs[2]._size = { 32, 32 }; layerDescs[3]._size = { 24, 24 }; neo::PredictiveHierarchy ph; ph.createRandom(cs, prog, { 64, 64 }, layerDescs, { -0.01f, 0.01f }, { 0.01f, 0.05f }, 0.1f, generator); float avgError = 1.0f; float avgErrorDecay = 0.1f; sf::RenderWindow window; window.create(sf::VideoMode(1024, 512), "MNIST Video Test"); vis::Plot plot; plot._curves.push_back(vis::Curve()); plot._curves[0]._name = "Squared Error"; std::uniform_int_distribution<int> digitDist(0, 59999); std::uniform_real<float> dist01(0.0f, 1.0f); sf::RenderTexture rt; rt.create(64, 64); sf::Image digit0; sf::Texture digit0Tex; sf::Image digit1; sf::Texture digit1Tex; sf::Image pred; sf::Texture predTex; digit0.create(28, 28); digit1.create(28, 28); pred.create(rt.getSize().x, rt.getSize().y); const float boundingSize = (64 - 28) / 2; const float center = 32; const float minimum = center - boundingSize; const float maximum = center + boundingSize; float avgError2 = 1.0f; const float avgError2Decay = 0.01f; std::vector<float> prediction(64 * 64, 0.0f); for (int iter = 0; iter < 10000; iter++) { // Select digit indices int d0 = digitDist(generator); int d1 = digitDist(generator); // Load digits Image img0, img1; loadMNISTimage(fromFile, d0, img0); loadMNISTimage(fromFile, d1, img1); for (int x = 0; x < digit0.getSize().x; x++) for (int y = 0; y < digit0.getSize().y; y++) { int index = x + y * digit0.getSize().x; sf::Color c = sf::Color::White; c.a = img0._intensities[index]; digit0.setPixel(x, y, c); } digit0Tex.loadFromImage(digit0); for (int x = 0; x < digit1.getSize().x; x++) for (int y = 0; y < digit1.getSize().y; y++) { int index = x + y * digit1.getSize().x; sf::Color c = sf::Color::White; c.a = img1._intensities[index]; digit1.setPixel(x, y, c); } digit1Tex.loadFromImage(digit1); sf::Vector2f vel0(dist01(generator) * 2.0f - 1.0f, dist01(generator) * 2.0f - 1.0f); sf::Vector2f vel1(dist01(generator) * 2.0f - 1.0f, dist01(generator) * 2.0f - 1.0f); sf::Vector2f pos0(dist01(generator) * (maximum - minimum) + minimum, dist01(generator) * (maximum - minimum) + minimum); sf::Vector2f pos1(dist01(generator) * (maximum - minimum) + minimum, dist01(generator) * (maximum - minimum) + minimum); float vel0mul = dist01(generator) * 6.0f / std::max(1.0f, std::sqrt(vel0.x * vel0.x + vel0.y + vel0.y)); vel0 *= vel0mul; float vel1mul = dist01(generator) * 6.0f / std::max(1.0f, std::sqrt(vel1.x * vel1.x + vel1.y + vel1.y)); vel1 *= vel1mul; // Render video for (int f = 0; f < 20; f++) { sf::Event windowEvent; while (window.pollEvent(windowEvent)) { switch (windowEvent.type) { case sf::Event::Closed: return 0; } } pos0 += vel0; pos1 += vel1; if (pos0.x < minimum) { pos0.x = minimum; vel0.x *= -1.0f; } else if (pos0.x > maximum) { pos0.x = maximum; vel0.x *= -1.0f; } if (pos0.y < minimum) { pos0.y = minimum; vel0.y *= -1.0f; } else if (pos0.y > maximum) { pos0.y = maximum; vel0.y *= -1.0f; } if (pos1.x < minimum) { pos1.x = minimum; vel1.x *= -1.0f; } else if (pos1.x > maximum) { pos1.x = maximum; vel1.x *= -1.0f; } if (pos1.y < minimum) { pos1.y = minimum; vel1.y *= -1.0f; } else if (pos1.y > maximum) { pos1.y = maximum; vel1.y *= -1.0f; } window.clear(); rt.clear(sf::Color::Black); sf::Sprite s0; s0.setTexture(digit0Tex); s0.setOrigin(28 / 2, 28 / 2); s0.setPosition(pos0); rt.draw(s0); sf::Sprite s1; s1.setTexture(digit1Tex); s1.setOrigin(28 / 2, 28 / 2); s1.setPosition(pos1); rt.draw(s1); rt.display(); // Get input image sf::Image res = rt.getTexture().copyToImage(); // Show RT const float scale = 4.0f; sf::Sprite s; s.setScale(scale, scale); s.setTexture(rt.getTexture()); window.draw(s); std::vector<float> input(64 * 64); // Train if (sf::Keyboard::isKeyPressed(sf::Keyboard::T)) { for (int x = 0; x < res.getSize().x; x++) for (int y = 0; y < res.getSize().y; y++) { input[x + y * 64] = prediction[x + y * 64]; } } else { const float predictionIncorporateRatio = 0.1f; for (int x = 0; x < res.getSize().x; x++) for (int y = 0; y < res.getSize().y; y++) { input[x + y * 64] = (1.0f - predictionIncorporateRatio) * res.getPixel(x, y).r / 255.0f + predictionIncorporateRatio * prediction[x + y * 64]; } } // Error float error = 0.0f; for (int x = 0; x < res.getSize().x; x++) for (int y = 0; y < res.getSize().y; y++) { error += std::pow(res.getPixel(x, y).r / 255.0f - prediction[x + y * 64], 2); } error /= res.getSize().x * res.getSize().y; avgError2 = (1.0f - avgError2Decay) * avgError2 + avgError2Decay * error; std::cout << "Squared Error: " << avgError2 << std::endl; cs.getQueue().enqueueWriteImage(inputImage, CL_TRUE, { 0, 0, 0 }, { 64, 64, 1 }, 0, 0, input.data()); ph.simStep(cs, inputImage); cs.getQueue().enqueueReadImage(ph.getPrediction(), CL_TRUE, { 0, 0, 0 }, { 64, 64, 1 }, 0, 0, prediction.data()); // Show prediction for (int x = 0; x < rt.getSize().x; x++) for (int y = 0; y < rt.getSize().y; y++) { sf::Color c = sf::Color::White; c.r = c.b = c.g = std::min(1.0f, std::max(0.0f, prediction[x + y * 64])) * 255.0f; pred.setPixel(x, y, c); } predTex.loadFromImage(pred); sf::Sprite sp; sp.setTexture(predTex); sp.setScale(scale, scale); sp.setPosition(window.getSize().x - scale * rt.getSize().x, 0); window.draw(sp); /*sf::Image sdr; sdr.create(prsdr.getLayerDescs().front()._width, prsdr.getLayerDescs().front()._height); for (int x = 0; x < sdr.getSize().x; x++) for (int y = 0; y < sdr.getSize().y; y++) { sf::Color c = sf::Color::White; c.r = c.g = c.b = prsdr.getLayers().front()._sdr.getHiddenState(x, y) * 255.0f; sdr.setPixel(x, y, c); } sf::Texture sdrTex; sdrTex.loadFromImage(sdr); sf::Sprite sdrS; sdrS.setTexture(sdrTex); sdrS.setPosition(0.0f, window.getSize().y - sdrTex.getSize().y * scale); sdrS.setScale(scale, scale); window.draw(sdrS);*/ window.display(); if (sf::Keyboard::isKeyPressed(sf::Keyboard::Escape)) return 0; } } /*sf::RenderTexture rt; rt.create(1024, 1024); sf::Texture lineGradientTexture; lineGradientTexture.loadFromFile("resources/lineGradient.png"); sf::Font tickFont; tickFont.loadFromFile("resources/arial.ttf"); plot.draw(rt, lineGradientTexture, tickFont, 1.0f, sf::Vector2f(0.0f, step), sf::Vector2f(0.0f, 1.0f), sf::Vector2f(128.0f, 128.0f), sf::Vector2f(500.0f, 0.1f), 2.0f, 3.0f, 1.5f, 3.0f, 20.0f, 6); rt.display(); rt.getTexture().copyToImage().saveToFile("plot.png");*/ return 0; }