int PxController::bound(Vector2f &n) {
n.x() = -n.x();
if(n.dot(v_) >= 0) {
return 1;
}
v_ += -2*(n.dot(v_))*n;
bound_locked_ = true;
std::cout << "----bound----- "<< std::endl;
std::cout << " n:" << n.x() << "," << n.y() << std::endl;
std::cout<< " v:" << v_.x() << "," << v_.y() << std::endl;
return 0;
}
float Attitude::getDip() const
{
Vector2f north = Vector2f::UnitX(); // parallel to x axis
Vector2f dip = getDipDirectionVector().head(
2); // he third element is 0 (working on horizontal plane)
float angle = acos(north.dot(dip)) * 180 / M_PI;
if (dip(1)
> 0.0) // if y component is postive we are in the 180 to 360 half-space
angle = 360 - angle;
return angle;
}
bool Sprite::manageCollision(Sprite *obj) {
if ( !strategy->execute(*this, *obj) ) return false;
int widthSum = frame->getWidth()/2 + obj->getFrame()->getWidth()/2;
if ( getDistance(obj) <= widthSum ) {
Vector2f velocity = getVelocity() - obj->getVelocity();
Vector2f distance = getPosition() - obj->getPosition();
if ( velocity.dot(distance) < 0 ) {
distance = distance.normalize();
Vector2f v1 = getVelocity();
v1 -= 2 * distance * getVelocity().dot(distance);
setVelocity(v1);
Vector2f v2 = obj->getVelocity();
v2 -= 2 * distance * obj->getVelocity().dot(distance);
obj->setVelocity(v2);
} // end of inner if
} // end of outer if
return true;
}
Vector4f*
Atmosphere::computeInscatterTable() const
{
// Rg - "ground radius"
// Rt - "transparent radius", i.e. radius of the atmosphere at some point
// where it is visually undetectable.
float Rg = planetRadius;
float Rg2 = Rg * Rg;
float Rt = shellRadius();
float Rt2 = Rt * Rt;
// Avoid numerical precision problems by choosing a first viewer
// position just *above* the planet surface.
float baseHeight = Rg * 1.0e-6f;
unsigned int sampleCount = HeightSamples * ViewAngleSamples * SunAngleSamples;
Vector4f* inscatter = new Vector4f[sampleCount];
for (unsigned int i = 0; i < HeightSamples; ++i)
{
float w = float(i) / float(HeightSamples);
float h = w * w * (Rt - Rg) + baseHeight;
float r = Rg + h;
float r2 = r * r;
cout << "layer " << i << ", height=" << h << "km\n";
Vector2f eye(0.0f, r);
for (unsigned int j = 0; j < ViewAngleSamples; ++j)
{
float v = float(j) / float(ViewAngleSamples - 1);
float mu = max(-1.0f, min(1.0f, toMu(v)));
float cosTheta = mu;
float sinTheta2 = 1.0f - cosTheta * cosTheta;
float sinTheta = sqrt(sinTheta2);
Vector2f view(sinTheta, cosTheta);
float pathLength;
float d = Rg2 - r2 * sinTheta2;
if (d > 0.0f && -r * cosTheta - sqrt(d) > 0.0f)
{
// Ray hits the planet
pathLength = -r * cosTheta - sqrt(Rg2 - r2 * sinTheta2);
}
else
{
// Ray hits the sky
pathLength = -r * cosTheta + sqrt(Rt2 - r2 * sinTheta2);
}
for (unsigned int k = 0; k < SunAngleSamples; ++k)
{
float w = float(k) / float(SunAngleSamples - 1);
float muS = toMuS(w);
float cosPhi = muS;
float sinPhi = sqrt(max(0.0f, 1.0f - cosPhi * cosPhi));
Vector2f sun(sinPhi, cosPhi);
float stepLength = pathLength / float(ScatteringIntegrationSteps);
Vector2f step = view * stepLength;
Vector3f rayleigh = Vector3f::Zero();
float mie = 0.0f;
for (unsigned int m = 0; m < ScatteringIntegrationSteps; ++m)
{
Vector2f x = eye + step * m;
float distanceToViewer = stepLength * m;
float rx2 = x.squaredNorm();
float rx = sqrt(rx2);
// Compute the transmittance along the path to the viewer
Vector3f viewPathTransmittance = transmittance(r, mu, distanceToViewer, *this);
// Compute the cosine and sine of the angle between the
// sun direction and zenith at the current sample.
float c = x.dot(sun) / rx;
float s2 = 1.0f - c * c;
// Compute the transmittance along the path to the sun
// and the total transmittance t.
Vector3f t;
if (Rg2 - rx2 * s2 < 0.0f || -rx * c - sqrt(Rg2 - rx2 * s2) < 0.0f)
{
// Compute the distance through the atmosphere
// in the direction of the sun
float sunPathLength = -rx * c + sqrt(Rt2 - rx2 * s2);
Vector3f sunPathTransmittance = transmittance(rx, c, sunPathLength, *this);
t = viewPathTransmittance.cwise() * sunPathTransmittance;
}
else
{
// Ray to sun intersects the planet; no inscattered
// light at this point.
t = Vector3f::Zero();
}
// Accumulate Rayleigh and Mie scattering
float hx = rx - Rg;
rayleigh += (exp(-hx / rayleighScaleHeight) * stepLength) * t;
mie += exp(-hx / mieScaleHeight) * stepLength * t.x();
}
unsigned int index = (i * ViewAngleSamples + j) * SunAngleSamples + k;
inscatter[index] << rayleigh.cwise() * rayleighCoeff,
mie * mieCoeff;
if (i == HeightSamples - 1 && k == 0)
{
cout << acos(muS) * 180.0/M_PI << ", "
<< acos(mu) * 180.0/M_PI << ", "
<< inscatter[index].transpose() << endl;
}
#if 0
// Emit warnings about NaNs in scatter table
if (isNaN(rayleigh.x()))
{
cout << "NaN in inscatter table at (" << k << ", " << j << ", " << i << ")\n";
}
#endif
}
}
}
return inscatter;
}
inline float eigenCross(const Vector2f &v1, const Vector2f &v2)
{
return v1.dot(Vector2f(v2.y(),-v2.x()));
}
void NPCFreefall_DZ::onUpdate(float dt)
{
// obtain spinal cord
SpinalCord* spinalCord = getNPC()->getSpinalCord();
spinalCord->reset();
// update target position
Matrix4f catToyPose = getNPC()->getCatToy()->getCurrentPose();
_targetPos.set( catToyPose[3][0], catToyPose[3][1], catToyPose[3][2] );
// jump only when the wind is right
Vector2f pos = Vector2f(catToyPose[3][0], catToyPose[3][2]);
pos.normalize();
NxVec3 wind = getNPC()->getJumper()->getScene()->getWindAtPoint(NxVec3(catToyPose[3][0], catToyPose[3][1], catToyPose[3][2]));
wind.normalize();
Vector2f wind2(wind.x, wind.z);
float wind_angle = 0.0;
if (wind.magnitude() < 1.5f) {
wind_angle = 1.0f;
} else {
wind2.normalize();
wind_angle = pos.dot(wind2);
}
// if my character is still roaming
if (wind_angle >= 0.8f && catToyPose[3][1] >= 300.0f) _timeUntilJump -= dt;
//getCore()->logMessage("%s has %2.4f seconds to jump", getNPC()->getNPCName(), _timeUntilJump);
if( _timeUntilJump <= 0.0f && getNPC()->getJumper()->getPhase() == ::jpRoaming )
{
Vector3f pos = Vector3f(catToyPose[3][0], catToyPose[3][1], catToyPose[3][2]);
// if npc on airplane
if( getNPC()->getJumper()->getAirplane() != NULL )
{
// just jump!
spinalCord->phase = true;
return;
}
// if npc at the exit point and ready to jump
else if( _positionIsSucceed && _directionIsSucceed )
{
// jump!
spinalCord->phase = true;
return;
}
// if npc at the exit point and not surely ready to clear jump
else
{
if( !_positionIsSucceed )
{
// move to abyss
Matrix4f jumpPose = getNPC()->getCatToy()->getJumpPose();
Vector3f jumpPos( jumpPose[3][0], jumpPose[3][1], jumpPose[3][2] );
call( new NPCMove( getNPC(), jumpPos ) );
_positionIsSucceed = true;
return;
}
if( !_directionIsSucceed )
{
// jumper absolute orientation
Vector3f jumperAt = getNPC()->getJumper()->getClump()->getFrame()->getAt();
jumperAt.normalize();
// direction of cat toy jump
Matrix4f jumpPose = getNPC()->getCatToy()->getJumpPose();
Vector3f targetDir( jumpPose[2][0], 0.0f, jumpPose[2][2] );
targetDir.normalize();
// angle to target
Vector3f atH = jumperAt; atH[1] = 0; atH.normalize();
Vector3f dirH = targetDir; dirH[1] = 0; dirH.normalize();
float targetAngle = ::calcAngle( dirH, atH, Vector3f( 0,1,0 ) );
if( fabs( targetAngle ) < 1.0f )
{
_directionIsSucceed = true;
return;
}
else
{
// turn jumper by AI algo
float aiRotationVel = 180.0f;
float aiRotationAngle = sgn( targetAngle ) * aiRotationVel * dt;
if( fabs( aiRotationAngle ) > fabs( targetAngle ) ) aiRotationAngle = targetAngle;
getNPC()->getJumper()->getClump()->getFrame()->rotateRelative( Vector3f( 0,1,0 ), aiRotationAngle );
}
}
}
}
// else, turn & track towards the target
else if( getNPC()->getJumper()->getPhase() == ::jpFreeFalling )
{
// jumper absolute orientation
Vector3f jumperAt = getNPC()->getJumper()->getClump()->getFrame()->getAt();
jumperAt.normalize();
Vector3f jumperRight = getNPC()->getJumper()->getClump()->getFrame()->getRight();
jumperRight.normalize();
Vector3f jumperUp = getNPC()->getJumper()->getClump()->getFrame()->getUp();
jumperUp.normalize();
// retrieve current jumper position
Vector3f jumperPos = getNPC()->getJumper()->getClump()->getFrame()->getPos();
jumperPos += Vector3f( 0, jumperRoamingSphereSize, 0 );
// wingsuit
bool wingsuit = database::Suit::getRecord(getNPC()->getJumper()->getVirtues()->equipment.suit.id)->wingsuit;
// leveling
// wlo - when npc higher
//if (!wingsuit) {
// if (jumperPos[1] - _targetPos[1] > 1500.0f) {
// spinalCord->wlo = true;
// } else if (jumperPos[1] - _targetPos[1] < -1500.0f) {
// spinalCord->hook = true;
// }
//}
// direction to target
Vector3f targetDir = _targetPos - jumperPos;
float distanceToTarget = Vector3f( targetDir[0], 0, targetDir[2] ).length();
targetDir.normalize();
// landing too far away? let's deploy in order to return safely
// glide ratio is based on canopy size
database::Canopy *canopy = database::Canopy::getRecord(getNPC()->getJumper()->getVirtues()->equipment.canopy.id);
Vector3f pos = getNPC()->getJumper()->getClump()->getFrame()->getPos();
float glide = canopy->square / 150.0f;
float alt = pos[1];
pos[1] = 0;
bool farEnough = pos.length() >= alt*glide;
//getCore()->logMessage("alt: %2.5f; dst: %2.5f; coverage: %2.5f", alt, pos.length(), alt*glide);
// deploy now?
if ((alt <= 85000.0f || farEnough) && alt <= 150000.0f) {
if (getNPC()->getJumper()->getCanopySimulator()) {
//getCore()->logMessage("npc pull. Far enough: %d", (int)farEnough);
spinalCord->phase = true;
spinalCord->modifier = wingsuit;
} else {
//getCore()->logMessage("npc pull reserve. Far enough: %d", (int)farEnough);
spinalCord->pullReserve = true;
spinalCord->modifier = wingsuit;
}
}
// if toy tracking modifier is on
if( wingsuit /* getNPC()->getCatToy()->getModifier()*/ )
{
// sum up target direction
Vector3f targetFlatAt(
getNPC()->getCatToy()->getCurrentPose()[2][0],
0,
getNPC()->getCatToy()->getCurrentPose()[2][2]
);
/*getCore()->logMessage(
"POS: %3.2f %3.2f %3.2f AT : %3.2f 0.0 %3.2f",
getNPC()->getCatToy()->getCurrentPose()[3][0],
getNPC()->getCatToy()->getCurrentPose()[3][1],
getNPC()->getCatToy()->getCurrentPose()[3][2],
targetFlatAt[0], targetFlatAt[2]
);*/
targetFlatAt.normalize();
targetDir += targetFlatAt * 3;
targetDir.normalize();
}
// horizontal steering
float minAngle = 5.0f;
float minValue = 0.0f;
float maxAngle = 45.0f;
float maxValue = 1.0f;
// angle to target
Vector3f atH = jumperAt;
// headdown
if (getNPC()->getJumper()->getDefaultPose() == 1) {
atH = jumperUp;
minAngle = 25.0f;
maxValue = 0.1f;
// sitfly
} else if (getNPC()->getJumper()->getDefaultPose() == 2) {
atH = jumperUp;
atH[2] *= -1;
//minAngle = 25.0f;
maxValue = 0.1f;
}
atH[1] = 0; atH.normalize();
Vector3f dirH = targetDir; dirH[1] = 0; dirH.normalize();
float targetAngle = ::calcAngle( dirH, atH, Vector3f( 0,1,0 ) );
// inclination angle relative to the horizont
float horizontalAngle = -1 * ::calcAngle( atH, jumperUp, jumperRight );
// horizontal steering
float factor = ( fabs( targetAngle ) - minAngle ) / ( maxAngle - minAngle );
factor = ( factor < 0 ) ? 0 : ( ( factor > 1 ) ? 1 : factor );
float impulse = minValue * ( 1 - factor ) + maxValue * factor;
if (wingsuit) impulse = 0.0f;
// smooth impulse
impulse = pow( impulse, 1.25f );
if (alt <= 130000.0f) impulse = 0.0f;
// apply impulse
if( targetAngle < 0 )
{
spinalCord->right = impulse;
}
else
{
spinalCord->left = impulse;
}
// relation btw desired inclination and target angle
minAngle = 5.0f;
minValue = 25.0f;
maxAngle = 90.0f;
maxValue = 0.0f;
factor = ( fabs( targetAngle ) - minAngle ) / ( maxAngle - minAngle );
factor = ( factor < 0 ) ? 0 : ( ( factor > 1 ) ? 1 : factor );
float desiredInclination = minValue * ( 1 - factor ) + maxValue * factor;
// inclination difference
float inclinationDifference = desiredInclination - horizontalAngle;
// vertical steering
minAngle = 0.0f;
minValue = 0.0f;
maxAngle = 30.0f;
maxValue = 1.0f;
factor = ( fabs( inclinationDifference ) - minAngle ) / ( maxAngle - minAngle );
factor = ( factor < 0 ) ? 0 : ( ( factor > 1 ) ? 1 : factor );
impulse = minValue * ( 1 - factor ) + maxValue * factor;
if (!wingsuit && distanceToTarget > 2500.0f && alt > 130000.0f) {
if( inclinationDifference < 0 )
{
spinalCord->down += impulse;
if (spinalCord->down > 1.0f) spinalCord->down = 1.0f;
}
else
{
spinalCord->up += impulse;
if (spinalCord->up > 1.0f) spinalCord->up = 1.0f;
}
}
// tracking modifier
if( fabs( targetAngle ) < 90.0f && distanceToTarget > 2500.0f )
{
spinalCord->modifier = 0.0f;
spinalCord->trigger_modifier = false;
}
else
{
spinalCord->modifier = 0.0f;
spinalCord->trigger_modifier = false;
}
if (wingsuit || alt <= 130000.0f) spinalCord->modifier = 1.0f;
if (wingsuit || alt <= 130000.0f) spinalCord->trigger_modifier = true;
return;
// force tracking in several condition
if( getNPC()->getCatToy()->getModifier() )
{
// not a skydiving?
if( !getNPC()->getJumper()->getCanopySimulator()->getGearRecord()->skydiving )
{
// imitate cat toy modifier
spinalCord->modifier = getNPC()->getCatToy()->getModifier();
}
else
{
Vector3f cattoyFaceH( catToyPose[2][0], 0, catToyPose[2][2] );
cattoyFaceH.normalize();
Vector3f npcFaceH = getNPC()->getJumper()->getClump()->getFrame()->getAt();
npcFaceH[1] = 0;
npcFaceH.normalize();
//if( Vector3f::dot( dirH, cattoyFaceH ) > 0.85f ) spinalCord->modifier = true;
}
}
}
}
