///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
void btRigidBody::applyDamping(btScalar timeStep)
{
//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
//#define USE_OLD_DAMPING_METHOD 1
#ifdef USE_OLD_DAMPING_METHOD
m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
#else
m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep);
m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep);
#endif
if (m_additionalDamping)
{
//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
(m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
{
m_angularVelocity *= m_additionalDampingFactor;
m_linearVelocity *= m_additionalDampingFactor;
}
btScalar speed = m_linearVelocity.length();
if (speed < m_linearDamping)
{
btScalar dampVel = btScalar(0.005);
if (speed > dampVel)
{
btVector3 dir = m_linearVelocity.normalized();
m_linearVelocity -= dir * dampVel;
} else
{
m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
}
}
btScalar angSpeed = m_angularVelocity.length();
if (angSpeed < m_angularDamping)
{
btScalar angDampVel = btScalar(0.005);
if (angSpeed > angDampVel)
{
btVector3 dir = m_angularVelocity.normalized();
m_angularVelocity -= dir * angDampVel;
} else
{
m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
}
}
}
}
void btFluidSphSurfaceTensionForce::computeColorFieldGradient(const btFluidSphParametersGlobal& FG, btFluidSph* fluid,
const btAlignedObjectArray<btFluidSphNeighbors>& neighborTables,
const btAlignedObjectArray<btScalar>& invDensity)
{
BT_PROFILE("btFluidSphSolverDefault::computeColorFieldGradient()");
int numParticles = fluid->numParticles();
const btFluidSphParametersLocal& FL = fluid->getLocalParameters();
const btFluidSortingGrid& grid = fluid->getGrid();
btFluidParticles& particles = fluid->internalGetParticles();
for(int i = 0; i < numParticles; ++i) m_colorFieldGradient[i].setValue(0, 0, 0);
for(int group = 0; group < btFluidSortingGrid::NUM_MULTITHREADING_GROUPS; ++group)
{
const btAlignedObjectArray<int>& multithreadingGroup = grid.internalGetMultithreadingGroup(group);
if( !multithreadingGroup.size() ) continue;
//If using multiple threads, this needs to be moved to a separate function
{
for(int cell = 0; cell < multithreadingGroup.size(); ++cell)
computeColorFieldGradientInCellSymmetric(FG, multithreadingGroup[cell], grid, particles,
neighborTables, invDensity, m_colorFieldGradient);
}
}
#ifdef USE_SPIKY_KERNEL_GRADIENT
for(int i = 0; i < numParticles; ++i) m_colorFieldGradient[i] *= FG.m_sphSmoothRadius * FL.m_sphParticleMass * FG.m_spikyKernGradCoeff;
#else
const btScalar poly6KernGradCoeff = btScalar(-945.0) / ( btScalar(32.0) * SIMD_PI * btPow(FG.m_sphSmoothRadius, 9) );
for(int i = 0; i < numParticles; ++i) m_colorFieldGradient[i] *= FG.m_sphSmoothRadius * FL.m_sphParticleMass * poly6KernGradCoeff;
#endif
}
//need to make a proper evaluation function
//get the best CPG function, delete the rest of CPGs and clear the m_birdCPGs array
void BirdOptimizer::evaluateCurrentGenerationBirds() {
m_perturb_scaler = btExp(-btPow(((m_numGeneration - 1) * 2.8f/50.f + 1.5f), 1.2f));
if (m_numGeneration == 0)
return;
//choose the best CPG
float minEnergy = FLT_MAX;
int minIndex = -1;
for (int ii = 0; ii < m_birdTrajectoryData.size(); ++ii) {
float energy =
calculateTimeToHitGround(m_birdTrajectoryData[ii])
+ calculateTotalUpTime(m_birdTrajectoryData[ii])
+ calculateEnergy(m_birdTrajectoryData[ii])/1500.0;
std::cout << "\tenergy: " << energy << std::endl;
if (energy < minEnergy)
{
minEnergy = energy;
minIndex = ii;
}
}
std::cout << "---GENERATION: " << m_numGeneration << " :: "
<< "min_energy: " << minEnergy
<< ", index: " << minIndex
<< ", scaler: " << m_perturb_scaler
<< std::endl;
m_currentBestInfo = m_birdInfos[minIndex];
// Save bird configuration.
proto::BirdOptimizerResult* result = m_result_data.add_result();
result->set_cum_energy(minEnergy);
result->mutable_bird()->CopyFrom(*m_birdInfos[minIndex]);
{
std::ofstream ofs("C:\\Users\\k\\bird_data.pbdata", std::ios::out | std::ios::trunc | std::ios::binary);
ofs << m_result_data.SerializeAsString();
}
{
std::ofstream ofs("C:\\Users\\k\\bird_data.txt", std::ios::out | std::ios::trunc);
ofs << m_result_data.DebugString();
}
for (int ii = 0; ii < m_birdTrajectoryData.size(); ++ii) {
delete m_birdTrajectoryData[ii];
}
m_birdTrajectoryData.clear();
for (int ii = 0; ii < m_birdInfos.size(); ii++) {
if (m_birdInfos[ii] == m_currentBestInfo) continue;
delete m_birdInfos[ii];
}
m_birdInfos.clear();
}
unsigned int btPolarDecomposition::decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const
{
// Use the 'u' and 'h' matrices for intermediate calculations
u = a;
h = a.inverse();
for (unsigned int i = 0; i < m_maxIterations; ++i)
{
const btScalar h_1 = p1_norm(h);
const btScalar h_inf = pinf_norm(h);
const btScalar u_1 = p1_norm(u);
const btScalar u_inf = pinf_norm(u);
const btScalar h_norm = h_1 * h_inf;
const btScalar u_norm = u_1 * u_inf;
// The matrix is effectively singular so we cannot invert it
if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm))
break;
const btScalar gamma = btPow(h_norm / u_norm, 0.25f);
const btScalar inv_gamma = 1.0 / gamma;
// Determine the delta to 'u'
const btMatrix3x3 delta = (u * (gamma - 2.0) + h.transpose() * inv_gamma) * 0.5;
// Update the matrices
u += delta;
h = u.inverse();
// Check for convergence
if (p1_norm(delta) <= m_tolerance * u_1)
{
h = u.transpose() * a;
h = (h + h.transpose()) * 0.5;
return i;
}
}
// The algorithm has failed to converge to the specified tolerance, but we
// want to make sure that the matrices returned are in the right form.
h = u.transpose() * a;
h = (h + h.transpose()) * 0.5;
return m_maxIterations;
}
void btFluidHfBuoyantConvexShape::generateShape(btScalar collisionRadius, btScalar volumeEstimationRadius)
{
m_voxelPositions.resize(0);
btVoronoiSimplexSolver simplexSolver;
btTransform voxelTransform = btTransform::getIdentity();
btVector3 aabbMin, aabbMax;
getAabb( btTransform::getIdentity(), aabbMin, aabbMax ); //AABB of the convex shape
//Generate voxels for collision
{
btSphereShape collisionShape(collisionRadius);
const btScalar collisionDiameter = btScalar(2.0) * collisionRadius;
btVector3 numVoxels = (aabbMax - aabbMin) / collisionDiameter;
for(int i = 0; i < ceil( numVoxels.x() ); i++)
{
for(int j = 0; j < ceil( numVoxels.y() ); j++)
{
for(int k = 0; k < ceil( numVoxels.z() ); k++)
{
btVector3 voxelPosition = aabbMin + btVector3(i * collisionDiameter, j * collisionDiameter, k * collisionDiameter);
voxelTransform.setOrigin(voxelPosition);
if( intersect(&simplexSolver, btTransform::getIdentity(), voxelTransform, m_convexShape, &collisionShape) )
m_voxelPositions.push_back(voxelPosition);
}
}
}
const bool CENTER_VOXELS_AABB_ON_ORIGIN = true;
if(CENTER_VOXELS_AABB_ON_ORIGIN)
{
btVector3 diameter(collisionDiameter, collisionDiameter, collisionDiameter);
btVector3 voxelAabbMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
btVector3 voxelAabbMax(-BT_LARGE_FLOAT, -BT_LARGE_FLOAT, -BT_LARGE_FLOAT);
for(int i = 0; i < m_voxelPositions.size(); ++i)
{
voxelAabbMin.setMin(m_voxelPositions[i] - diameter);
voxelAabbMax.setMax(m_voxelPositions[i] + diameter);
}
btVector3 offset = (voxelAabbMax - voxelAabbMin)*btScalar(0.5) - voxelAabbMax;
for(int i = 0; i < m_voxelPositions.size(); ++i) m_voxelPositions[i] += offset;
}
}
//Estimate volume with smaller spheres
btScalar estimatedVolume;
{
btSphereShape volumeEstimationShape(volumeEstimationRadius);
int numCollidingVoxels = 0;
const btScalar estimationDiameter = btScalar(2.0) * volumeEstimationRadius;
btVector3 numEstimationVoxels = (aabbMax - aabbMin) / estimationDiameter;
for(int i = 0; i < ceil( numEstimationVoxels.x() ); i++)
{
for(int j = 0; j < ceil( numEstimationVoxels.y() ); j++)
{
for(int k = 0; k < ceil( numEstimationVoxels.z() ); k++)
{
btVector3 voxelPosition = aabbMin + btVector3(i * estimationDiameter, j * estimationDiameter, k * estimationDiameter);
voxelTransform.setOrigin(voxelPosition);
if( intersect(&simplexSolver, btTransform::getIdentity(), voxelTransform, m_convexShape, &volumeEstimationShape) )
++numCollidingVoxels;
}
}
}
//Although the voxels are spherical, it is better to use the volume of a cube
//for volume estimation. Since convex shapes are completely solid and the voxels
//are generated by moving along a cubic lattice, using the volume of a sphere
//would result in gaps. Comparing the volume of a cube with edge length 2(8 m^3)
//and the volume of a sphere with diameter 2(~4 m^3), the estimated volume would
//be off by about 1/2.
btScalar volumePerEstimationVoxel = btPow( estimationDiameter, btScalar(3.0) );
//btScalar volumePerEstimationVoxel = btScalar(4.0/3.0) * SIMD_PI * btPow( volumeEstimationRadius, btScalar(3.0) );
estimatedVolume = static_cast<btScalar>(numCollidingVoxels) * volumePerEstimationVoxel;
}
m_volumePerVoxel = estimatedVolume / static_cast<btScalar>( getNumVoxels() );
m_totalVolume = estimatedVolume;
m_collisionRadius = collisionRadius;
}
void CDynamics3DMagnetismPlugin::Update() {
if(m_vecDipoles.size() < 2) {
/* Nothing to do */
return;
}
for(std::vector<SMagneticDipole>::iterator itDipole0 = std::begin(m_vecDipoles);
itDipole0 != (std::end(m_vecDipoles) - 1);
++itDipole0) {
for(std::vector<SMagneticDipole>::iterator itDipole1 = std::next(itDipole0, 1);
itDipole1 != std::end(m_vecDipoles);
++itDipole1) {
const btTransform& cTransformDipole0 = itDipole0->Offset * itDipole0->Body->GetTransform();
const btTransform& cTransformDipole1 = itDipole1->Offset * itDipole1->Body->GetTransform();
const btVector3& cPositionDipole0 = cTransformDipole0.getOrigin();
const btVector3& cPositionDipole1 = cTransformDipole1.getOrigin();
/* calculate the distance between the two magnetic bodies */
btScalar fDistance = cPositionDipole0.distance(cPositionDipole1);
/* optimization - don't calculate magnetism for dipoles more than m_fMaxDistance apart */
if(fDistance > m_fMaxDistance) {
continue;
}
/* perform Barnes-hut algorithm */
/* calculate the normalized seperation between the two dipoles, pointing from Dipole1 to Dipole0*/
const btVector3& cNormalizedSeparation =
btVector3(cPositionDipole0 - cPositionDipole1) / fDistance;
/* calculate the rotated field of dipole 0 */
const btVector3& cRotatedFieldDipole0 =
itDipole0->Body->GetTransform().getBasis() * itDipole0->GetField();
/* calculate the rotated field of dipole 1 */
const btVector3& cRotatedFieldDipole1 =
itDipole1->Body->GetTransform().getBasis() * itDipole1->GetField();
/* We now have cRotatedFieldDipole0 and cRotatedFieldDipole1 as the magnetic moments
(i.e., m0, m1), cNormalizedSeparation as the direction unit vector from Dipole 1
to Dipole 0 (i.e., n), fDistance is the scalar distance between the dipoles (i.e.,
d), and B0 is the magnetic flux density at Dipole 0.
B0 = u0/4pi * [3n(n.m1) - m1] / d^3
T0 = m0 * B0
= u0.4pi/d^3 * [3 (m1.n)(m0 * n) - m0 * m1]
F0 = grad(m0.B0)
= u0.4pi/d^4 * [-15n(m0.n)(m1.n) + 3n(m0.m1) + 3(m0(m1.n)+m1(m0.n))]
*/
/* calculate the intermediate cross and dot products */
const btVector3& cCrossProduct01 = cRotatedFieldDipole0.cross(cRotatedFieldDipole1);
const btVector3& cCrossProduct0 = cRotatedFieldDipole0.cross(cNormalizedSeparation);
const btVector3& cCrossProduct1 = cRotatedFieldDipole1.cross(cNormalizedSeparation);
btScalar fDotProduct01 = cRotatedFieldDipole0.dot(cRotatedFieldDipole1);
btScalar fDotProduct0 = cRotatedFieldDipole0.dot(cNormalizedSeparation);
btScalar fDotProduct1 = cRotatedFieldDipole1.dot(cNormalizedSeparation);
/* calculate the magnetic force and torque */
const btVector3& cTorque0 =
((3 * fDotProduct1 * cCrossProduct0) - cCrossProduct01) *
m_fForceConstant / btPow(fDistance, 3);
const btVector3& cTorque1 =
((3 * fDotProduct0 * cCrossProduct1) + cCrossProduct01) *
m_fForceConstant / btPow(fDistance, 3);
const btVector3& cForce = (m_fForceConstant / btPow(fDistance, 4)) *
((-15 * cNormalizedSeparation * fDotProduct1 * fDotProduct0) +
(3 * cNormalizedSeparation * fDotProduct01) +
(3 * (fDotProduct1 * cRotatedFieldDipole0 + fDotProduct0 * cRotatedFieldDipole1)));
/* apply torques and forces to the bodies */
itDipole0->Body->ApplyForce(cForce, (itDipole0->Offset).getOrigin());
itDipole0->Body->ApplyTorque(cTorque0);
itDipole1->Body->ApplyForce(-cForce, (itDipole1->Offset).getOrigin());
itDipole1->Body->ApplyTorque(cTorque1);
}
}
}
