void peano::integration::partitioncoupling::builtin::records::ForceTorquePacked::toString (std::ostream& out) const { out << "("; out << "_translationalForce:["; for (int i = 0; i < 3-1; i++) { out << getTranslationalForce(i) << ","; } out << getTranslationalForce(3-1) << "]"; out << ","; out << "_torque:["; for (int i = 0; i < 3-1; i++) { out << getTorque(i) << ","; } out << getTorque(3-1) << "]"; out << ")"; }
void PhysShape::pack(BitStream* stream) { if (stream->writeFlag(isEnabled())) { bool active = stream->writeFlag(isActive()); if (!active) { bool toInactive = active!=mPrevActive; mPrevActive = active; if (!stream->writeFlag(toInactive)) return; } else mPrevActive = true; mathWrite(*stream, getPosition()); mathWrite(*stream, getRotation()); mathWrite(*stream, getForce()); mathWrite(*stream, getTorque()); mathWrite(*stream, getLinVelocity()); mathWrite(*stream, getAngVelocity()); } }
peano::integration::partitioncoupling::builtin::records::ForceTorque peano::integration::partitioncoupling::builtin::records::ForceTorquePacked::convert() const{ return ForceTorque( getTranslationalForce(), getTorque() ); }
double Motor::getConsumption() const { return getConsumption(getFrequency(), getTorque())*getFrequency()*getTorque()/1000.0*2.0*M_PI; }
void TimeStep::operator()() { if( numberOfSubIterations_ == 1 ) { forceEvaluationFunc_(); collisionResponse_.timestep( timeStepSize_ ); synchronizeFunc_(); } else { // during the intermediate time steps of the collision response, the currently acting forces // (interaction forces, gravitational force, ...) have to remain constant. // Since they are reset after the call to collision response, they have to be stored explicitly before. // Then they are set again after each intermediate step. // generate map from all known bodies (process local) to total forces/torques // this has to be done on a block-local basis, since the same body could reside on several blocks from this process using BlockID_T = domain_decomposition::IBlockID::IDType; std::map< BlockID_T, std::map< walberla::id_t, std::array< real_t, 6 > > > forceTorqueMap; for( auto blockIt = blockStorage_->begin(); blockIt != blockStorage_->end(); ++blockIt ) { BlockID_T blockID = blockIt->getId().getID(); auto& blockLocalForceTorqueMap = forceTorqueMap[blockID]; // iterate over local and remote bodies and store force/torque in map for( auto bodyIt = pe::BodyIterator::begin(*blockIt, bodyStorageID_); bodyIt != pe::BodyIterator::end(); ++bodyIt ) { auto & f = blockLocalForceTorqueMap[ bodyIt->getSystemID() ]; const auto & force = bodyIt->getForce(); const auto & torque = bodyIt->getTorque(); f = {{force[0], force[1], force[2], torque[0], torque[1], torque[2] }}; } } // perform pe time steps const real_t subTimeStepSize = timeStepSize_ / real_c( numberOfSubIterations_ ); for( uint_t i = 0; i != numberOfSubIterations_; ++i ) { // in the first iteration, forces are already set if( i != 0 ) { for( auto blockIt = blockStorage_->begin(); blockIt != blockStorage_->end(); ++blockIt ) { BlockID_T blockID = blockIt->getId().getID(); auto& blockLocalForceTorqueMap = forceTorqueMap[blockID]; // re-set stored force/torque on bodies for( auto bodyIt = pe::BodyIterator::begin(*blockIt, bodyStorageID_); bodyIt != pe::BodyIterator::end(); ++bodyIt ) { const auto f = blockLocalForceTorqueMap.find( bodyIt->getSystemID() ); if( f != blockLocalForceTorqueMap.end() ) { const auto & ftValues = f->second; bodyIt->addForce ( ftValues[0], ftValues[1], ftValues[2] ); bodyIt->addTorque( ftValues[3], ftValues[4], ftValues[5] ); } } } } // evaluate forces (e.g. lubrication forces) forceEvaluationFunc_(); collisionResponse_.timestep( subTimeStepSize ); synchronizeFunc_(); } } }
void PhysicsBody::changed(ConstFieldMaskArg whichField, UInt32 origin, BitVector details) { Inherited::changed(whichField, origin, details); //Do not respond to changes that have a Sync origin if(origin & ChangedOrigin::Sync) { return; } if(whichField & WorldFieldMask) { if(_BodyID != 0) { dBodyDestroy(_BodyID); } if(getWorld() != NULL) { _BodyID = dBodyCreate(getWorld()->getWorldID()); } } if( ((whichField & PositionFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetPosition(_BodyID, getPosition().x(),getPosition().y(),getPosition().z()); } if( ((whichField & RotationFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dMatrix3 rotation; Vec4f v1 = getRotation()[0]; Vec4f v2 = getRotation()[1]; Vec4f v3 = getRotation()[2]; rotation[0] = v1.x(); rotation[1] = v1.y(); rotation[2] = v1.z(); rotation[3] = 0; rotation[4] = v2.x(); rotation[5] = v2.y(); rotation[6] = v2.z(); rotation[7] = 0; rotation[8] = v3.x(); rotation[9] = v3.y(); rotation[10] = v3.z(); rotation[11] = 0; dBodySetRotation(_BodyID, rotation); } if( ((whichField & QuaternionFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dQuaternion q; q[0]=getQuaternion().w(); q[1]=getQuaternion().x(); q[2]=getQuaternion().y(); q[3]=getQuaternion().z(); dBodySetQuaternion(_BodyID,q); } if( ((whichField & LinearVelFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetLinearVel(_BodyID, getLinearVel().x(),getLinearVel().y(),getLinearVel().z()); } if( ((whichField & AngularVelFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAngularVel(_BodyID, getAngularVel().x(),getAngularVel().y(),getAngularVel().z()); } if( ((whichField & ForceFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetForce(_BodyID, getForce().x(),getForce().y(),getForce().z()); } if( ((whichField & TorqueFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetTorque(_BodyID, getTorque().x(),getTorque().y(),getTorque().z()); } if( ((whichField & AutoDisableFlagFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAutoDisableFlag(_BodyID, getAutoDisableFlag()); } if( ((whichField & AutoDisableLinearThresholdFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAutoDisableLinearThreshold(_BodyID, getAutoDisableLinearThreshold()); } if( ((whichField & AutoDisableAngularThresholdFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAutoDisableAngularThreshold(_BodyID, getAutoDisableAngularThreshold()); } if( ((whichField & AutoDisableStepsFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAutoDisableSteps(_BodyID, getAutoDisableSteps()); } if( ((whichField & AutoDisableTimeFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAutoDisableTime(_BodyID, getAutoDisableTime()); } if( ((whichField & FiniteRotationModeFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetFiniteRotationMode(_BodyID, getFiniteRotationMode()); } if( ((whichField & FiniteRotationModeFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetFiniteRotationMode(_BodyID, getFiniteRotationMode()); } if( ((whichField & FiniteRotationAxisFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetFiniteRotationAxis(_BodyID, getFiniteRotationAxis().x(),getFiniteRotationAxis().y(),getFiniteRotationAxis().z()); } if( ((whichField & GravityModeFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetGravityMode(_BodyID, getGravityMode()); } if( ((whichField & LinearDampingFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetLinearDamping(_BodyID, getLinearDamping()); } if( ((whichField & AngularDampingFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAngularDamping(_BodyID, getAngularDamping()); } if( ((whichField & LinearDampingThresholdFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetLinearDampingThreshold(_BodyID, getLinearDampingThreshold()); } if( ((whichField & AngularDampingThresholdFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAngularDampingThreshold(_BodyID, getAngularDampingThreshold()); } if( ((whichField & MaxAngularSpeedFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { if(getMaxAngularSpeed() >= 0.0) { dBodySetMaxAngularSpeed(_BodyID, getMaxAngularSpeed()); } else { dBodySetMaxAngularSpeed(_BodyID, dInfinity); } } if( ((whichField & MassFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dMass TheMass; dBodyGetMass(_BodyID, &TheMass); TheMass.mass = getMass(); dBodySetMass(_BodyID, &TheMass); } if( ((whichField & MassCenterOfGravityFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { //dMass TheMass; //dBodyGetMass(_BodyID, &TheMass); ////TheMass.c[0] = getMassCenterOfGravity().x(); ////TheMass.c[1] = getMassCenterOfGravity().y(); ////TheMass.c[2] = getMassCenterOfGravity().z(); //Vec4f v1 = getMassInertiaTensor()[0]; //Vec4f v2 = getMassInertiaTensor()[1]; //Vec4f v3 = getMassInertiaTensor()[2]; //TheMass.I[0] = v1.x(); //TheMass.I[1] = v1.y(); //TheMass.I[2] = v1.z(); //TheMass.I[3] = getMassCenterOfGravity().x(); //TheMass.I[4] = v2.x(); //TheMass.I[5] = v2.y(); //TheMass.I[6] = v2.z(); //TheMass.I[7] = getMassCenterOfGravity().y(); //TheMass.I[8] = v3.x(); //TheMass.I[9] = v3.y(); //TheMass.I[10] = v3.z(); //TheMass.I[11] = getMassCenterOfGravity().z(); //dBodySetMass(_BodyID, &TheMass); } if( ((whichField & MassInertiaTensorFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dMass TheMass; dBodyGetMass(_BodyID, &TheMass); Vec4f v1 = getMassInertiaTensor()[0]; Vec4f v2 = getMassInertiaTensor()[1]; Vec4f v3 = getMassInertiaTensor()[2]; TheMass.I[0] = v1.x(); TheMass.I[1] = v1.y(); TheMass.I[2] = v1.z(); TheMass.I[3] = 0; TheMass.I[4] = v2.x(); TheMass.I[5] = v2.y(); TheMass.I[6] = v2.z(); TheMass.I[7] = 0; TheMass.I[8] = v3.x(); TheMass.I[9] = v3.y(); TheMass.I[10] = v3.z(); TheMass.I[11] = 0; dBodySetMass(_BodyID, &TheMass); } if( ((whichField & KinematicFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { if(getKinematic()) { dBodySetKinematic(_BodyID); } else { dBodySetDynamic(_BodyID); } } }