//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();
}
btScalar Tire::PacejkaFx(
btScalar sigma,
btScalar Fz,
btScalar dFz,
btScalar friction_coeff) const
{
const btScalar * p = coefficients;
// vertical shift
btScalar Sv = Fz * (p[PVX1] + p[PVX2] * dFz);
// horizontal shift
btScalar Sh = p[PHX1] + p[PHX2] * dFz;
// composite slip
btScalar S = sigma + Sh;
// slope at origin
btScalar K = Fz * (p[PKX1] + p[PKX2] * dFz) * btExp(-p[PKX3] * dFz);
// curvature factor
btScalar E = (p[PEX1] + p[PEX2] * dFz + p[PEX3] * dFz * dFz) * (1 - p[PEX4] * sgn(S));
// peak factor
btScalar D = Fz * (p[PDX1] + p[PDX2] * dFz);
// shape factor
btScalar C = p[PCX1];
// stiffness factor
btScalar B = K / (C * D);
// force
btScalar F = D * btSin(C * btAtan(B * S - E * (B * S - btAtan(B * S)))) + Sv;
// scale by surface friction
F *= friction_coeff;
return F;
}
///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)
// DrChat: bullet's old damping
/*
if (!btFuzzyZero(m_linearDamping) && !m_linearVelocity.fuzzyZero())
m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep);
if (!btFuzzyZero(m_angularDamping) && !m_angularVelocity.fuzzyZero())
m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep);
*/
if (!btFuzzyZero(m_linearDamping) && !m_linearVelocity.fuzzyZero()) {
m_linearVelocity *= btExp(-m_linearDamping * timeStep);
}
if (!btFuzzyZero(m_angularDamping) && !m_angularVelocity.fuzzyZero()) {
m_angularVelocity *= btExp(-m_angularDamping * timeStep);
}
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.));
}
}
}
}
