/** Returns the hit effect object to use when this objects hits something.
* \returns The hit effect object, or NULL if no hit effect should be played.
*/
HitEffect* Bowling::getHitEffect() const
{
if(m_has_hit_kart)
return new HitSFX(getXYZ(), "strike");
else
return new HitSFX(getXYZ(), "crash");
} // getHitEffect
void ITG3200::zeroCalibrate(unsigned int samples, unsigned int sampleDelayMS)
{
int16_t x_offset_temp = 0;
int16_t y_offset_temp = 0;
int16_t z_offset_temp = 0;
int16_t x,y,z;
x_offset = 0;
y_offset = 0;
z_offset = 0;
getXYZ(&x,&y,&z);//
for (int i = 0;i < samples;i++){
delay(sampleDelayMS);
getXYZ(&x,&y,&z);
x_offset_temp += x;
y_offset_temp += y;
z_offset_temp += z;
}
x_offset = abs(x_offset_temp)/samples;
y_offset = abs(y_offset_temp)/samples;
z_offset = abs(z_offset_temp)/samples;
if(x_offset_temp > 0)x_offset = -x_offset;
if(y_offset_temp > 0)y_offset = -y_offset;
if(z_offset_temp > 0)z_offset = -z_offset;
}
/** Creates the explosion physical effect, i.e. pushes the karts and ph
* appropriately. The corresponding visual/sfx needs to be added manually!
* \param kart_hit If non-NULL a kart that was directly hit.
* \param object If non-NULL a physical item that was hit directly.
* \param secondary_hits True if items that are not directly hit should
* also be affected.
*/
void Flyable::explode(AbstractKart *kart_hit, PhysicalObject *object,
bool secondary_hits)
{
// Apply explosion effect
// ----------------------
World *world = World::getWorld();
for ( unsigned int i = 0 ; i < world->getNumKarts() ; i++ )
{
AbstractKart *kart = world->getKart(i);
// If no secondary hits should be done, only hit the
// direct hit kart.
if(!secondary_hits && kart!=kart_hit)
continue;
// Handle the actual explosion. The kart that fired a flyable will
// only be affected if it's a direct hit. This allows karts to use
// rockets on short distance.
if( (m_owner!=kart || m_owner==kart_hit) && !kart->getKartAnimation())
{
// The explosion animation will register itself with the kart
// and will free it later.
ExplosionAnimation::create(kart, getXYZ(), kart==kart_hit);
if(kart==kart_hit && world->getTrack()->isArena())
{
world->kartHit(kart->getWorldKartId());
}
}
}
world->getTrack()->handleExplosion(getXYZ(), object, secondary_hits);
} // explode
/** Initialises the item. Note that m_distance_2 must be defined before calling
* this function, since it pre-computes some values based on this.
* \param type Type of the item.
* \param xyz Position of this item.
* \param normal Normal for this item.
*/
void Item::initItem(ItemType type, const Vec3 &xyz, const Vec3&normal)
{
ItemState::initItem(type, xyz, normal);
// Now determine in which quad this item is, and its distance
// from the center within this quad.
m_graph_node = Graph::UNKNOWN_SECTOR;
m_distance_from_center = 9999.9f;
m_avoidance_points[0] = NULL;
m_avoidance_points[1] = NULL;
// Check that Graph exist (it might not in battle mode without navmesh)
if (Graph::get())
{
Graph::get()->findRoadSector(xyz, &m_graph_node);
}
if (DriveGraph::get() && m_graph_node != Graph::UNKNOWN_SECTOR)
{
// Item is on drive graph. Pre-compute the distance from center
// of this item, which is used by the AI (mostly for avoiding items)
Vec3 distances;
DriveGraph::get()->spatialToTrack(&distances, getXYZ(), m_graph_node);
m_distance_from_center = distances.getX();
const DriveNode* dn = DriveGraph::get()->getNode(m_graph_node);
const Vec3& right = dn->getRightUnitVector();
// Give it 10% more space, since the kart will not always come
// parallel to the drive line.
Vec3 delta = right * sqrt(m_distance_2) * 1.3f;
m_avoidance_points[0] = new Vec3(getXYZ() + delta);
m_avoidance_points[1] = new Vec3(getXYZ() - delta);
}
} // initItem
void DynamicEntity::updateGround()
{
// ground is not a real dynamic object so we have to update it manually
Ground.XYZ.x = getXYZ().x;
Ground.XYZ.y = Game::Arena->getHeight(getXYZ().x, getXYZ().z) - GROUND_COLLISION_RADIUS;
Ground.XYZ.z = getXYZ().z;
// it's not handled by the corpus manager
Ground.update(0);
}
/** Returns the maximum height of the terrain at the current point. While
* generall the height is arbitrary (a skybox is not part of the physics and
* will therefore not be detected), it is important that a rubber ball does
* not end up on top of a tunnel.
* \param vertical_offset A vertical offset which is added to the current
* position of the kart in order to avoid tunneling effects (it could
* happen that the raycast down find the track since it uses the
* vertical offset, while the raycast up would hit under the track
* if the vertical offset is not used).
* \returns The height (Y coordinate) of the next terrain element found by
* a raycast up. If no terrain is found, it returns 99990
*/
float RubberBall::getMaxTerrainHeight(const Vec3 &vertical_offset) const
{
const TriangleMesh &tm = World::getWorld()->getTrack()->getTriangleMesh();
Vec3 to(getXYZ());
to.setY(10000.0f);
Vec3 hit_point;
const Material *material;
tm.castRay(getXYZ()+vertical_offset, to, &hit_point, &material);
return (material) ? hit_point.getY() : 99999.f;
} // getMaxTerrainHeight
RubberBall::RubberBall(AbstractKart *kart)
: Flyable(kart, PowerupManager::POWERUP_RUBBERBALL, 0.0f /* mass */),
TrackSector()
{
// For debugging purpose: pre-fix each debugging line with the id of
// the ball so that it's easy to collect all debug output for one
// particular ball only.
m_next_id++;
m_id = m_next_id;
// Don't let Flyable update the terrain information, since this object
// has to do it earlier than that.
setDoTerrainInfo(false);
float forw_offset = 0.5f*kart->getKartLength() + m_extend.getZ()*0.5f+5.0f;
createPhysics(forw_offset, btVector3(0.0f, 0.0f, m_speed*2),
new btSphereShape(0.5f*m_extend.getY()),
-70.0f /*gravity*/,
true /*rotates*/);
// Do not adjust the up velocity
setAdjustUpVelocity(false);
m_max_lifespan = 9999;
m_target = NULL;
m_aiming_at_target = false;
m_fast_ping = false;
// At the start the ball aims at quads till it gets close enough to the
// target:
m_height_timer = 0.0f;
m_interval = m_st_interval;
m_current_max_height = m_max_height;
m_ping_sfx = sfx_manager->createSoundSource("ball_bounce");
// Just init the previoux coordinates with some value that's not getXYZ()
m_previous_xyz = m_owner->getXYZ();
m_previous_height = 2.0f; //
// A negative value indicates that the timer is not active
m_delete_timer = -1.0f;
m_tunnel_count = 0;
LinearWorld *world = dynamic_cast<LinearWorld*>(World::getWorld());
// FIXME: what does the rubber ball do in case of battle mode??
if(!world) return;
computeTarget();
// initialises the current graph node
TrackSector::update(getXYZ());
TerrainInfo::update(getXYZ());
initializeControlPoints(m_owner->getXYZ());
} // RubberBall
/** Creates the explosion physical effect, i.e. pushes the karts and ph
* appropriately. The corresponding visual/sfx needs to be added manually!
* \param kart_hit If non-NULL a kart that was directly hit.
* \param object If non-NULL a physical item that was hit directly.
* \param secondary_hits True if items that are not directly hit should
* also be affected.
*/
void Flyable::explode(AbstractKart *kart_hit, PhysicalObject *object,
bool secondary_hits)
{
// Apply explosion effect
// ----------------------
World *world = World::getWorld();
for ( unsigned int i = 0 ; i < world->getNumKarts() ; i++ )
{
AbstractKart *kart = world->getKart(i);
// Don't explode teammates in team world
if (world->hasTeam() &&
world->getKartTeam(kart->getWorldKartId()) ==
world->getKartTeam(m_owner->getWorldKartId()))
continue;
if (kart->isGhostKart()) continue;
// If no secondary hits should be done, only hit the
// direct hit kart.
if(!secondary_hits && kart!=kart_hit)
continue;
// Handle the actual explosion. The kart that fired a flyable will
// only be affected if it's a direct hit. This allows karts to use
// rockets on short distance.
if( (m_owner!=kart || m_owner==kart_hit) && !kart->getKartAnimation())
{
// The explosion animation will register itself with the kart
// and will free it later.
ExplosionAnimation::create(kart, getXYZ(), kart==kart_hit);
if (kart == kart_hit)
{
world->kartHit(kart->getWorldKartId(),
m_owner->getWorldKartId());
if (m_owner->getController()->canGetAchievements())
{
if (m_owner->getWorldKartId() != kart->getWorldKartId())
PlayerManager::addKartHit(kart->getWorldKartId());
PlayerManager::increaseAchievement(AchievementsStatus::ALL_HITS, 1);
if (race_manager->isLinearRaceMode())
PlayerManager::increaseAchievement(AchievementsStatus::ALL_HITS_1RACE, 1);
}
}
}
}
Track::getCurrentTrack()->handleExplosion(getXYZ(), object,secondary_hits);
} // explode
void Acc::debugXYZ() {
double acc[3];
getXYZ(&acc[0], &acc[1], &acc[2]);
Serial.print("AcX = "); Serial.print(acc[0], 4);
Serial.print("\t | AcY = "); Serial.print(acc[1], 4);
Serial.print("\t | AcZ = "); Serial.println(acc[2], 4);
}
/** Virtual function called when the plunger hits something.
* The plunger is special in that it is not deleted when hitting an object.
* Instead it stays around (though not as a graphical or physical object)
* till the rubber band expires.
* \param kart Pointer to the kart hit (NULL if not a kart).
* \param obj Pointer to PhysicalObject object if hit (NULL otherwise).
*/
void Plunger::hit(Kart *kart, PhysicalObject *obj)
{
if(isOwnerImmunity(kart)) return;
RaceGUIBase* gui = World::getWorld()->getRaceGUI();
irr::core::stringw hit_message;
// pulling back makes no sense in battle mode, since this mode is not a race.
// so in battle mode, always hide view
if( m_reverse_mode || race_manager->isBattleMode() )
{
if(kart)
{
kart->blockViewWithPlunger();
hit_message += StringUtils::insertValues(getPlungerInFaceString(),
kart->getName().c_str(),
m_owner->getName().c_str()
).c_str();
gui->addMessage(hit_message, NULL, 3.0f, 40, video::SColor(255, 255, 255, 255), false);
}
m_keep_alive = 0;
// Make this object invisible by placing it faaar down. Note that if this
// objects is simply removed from the scene graph, it might be auto-deleted
// because the ref count reaches zero.
getNode()->setVisible(false);
World::getWorld()->getPhysics()->removeBody(getBody());
}
else
{
m_keep_alive = m_owner->getKartProperties()->getRubberBandDuration();
// Make this object invisible by placing it faaar down. Not that if this
// objects is simply removed from the scene graph, it might be auto-deleted
// because the ref count reaches zero.
scene::ISceneNode *node = getNode();
if(node)
{
node->setVisible(false);
}
World::getWorld()->getPhysics()->removeBody(getBody());
if(kart)
{
m_rubber_band->hit(kart);
return;
}
else if(obj)
{
Vec3 pos(obj->getBody()->getWorldTransform().getOrigin());
m_rubber_band->hit(NULL, &pos);
}
else
{
m_rubber_band->hit(NULL, &(getXYZ()));
}
}
} // hit
/*Function: Get the angular velocity and its unit is degree per second.*/
void ITG3200::getAngularVelocity(float *ax,float *ay,float *az)
{
int16_t x,y,z;
getXYZ(&x,&y,&z);
*ax = x/14.375;
*ay = y/14.375;
*az = z/14.375;
}
void MMA7660::getAcceleration(float *ax,float *ay,float *az)
{
int8_t x,y,z;
getXYZ(&x,&y,&z);
*ax = x/21.00;
*ay = y/21.00;
*az = z/21.00;
}
/** Updates the graphics model. Mainly set the graphical position to be the
* same as the physics position, but uses offsets to position and rotation
* for special gfx effects (e.g. skidding will turn the karts more).
* \param offset_xyz Offset to be added to the position.
* \param rotation Additional rotation.
*/
void Moveable::updateGraphics(float dt, const Vec3& offset_xyz,
const btQuaternion& rotation)
{
Vec3 xyz=getXYZ()+offset_xyz;
m_node->setPosition(xyz.toIrrVector());
btQuaternion r_all = getRotation()*rotation;
Vec3 hpr;
hpr.setHPR(r_all);
m_node->setRotation(hpr.toIrrHPR());
} // updateGraphics
bool MMA7660::getAcceleration(float *ax,float *ay,float *az)
{
int8_t x,y,z;
if(!getXYZ(&x,&y,&z))return 0;
*ax = x/21.00;
*ay = y/21.00;
*az = z/21.00;
return 1;
}
/** Virtual function called when the plunger hits something.
* The plunger is special in that it is not deleted when hitting an object.
* Instead it stays around (though not as a graphical or physical object)
* till the rubber band expires.
* \param kart Pointer to the kart hit (NULL if not a kart).
* \param obj Pointer to PhysicalObject object if hit (NULL otherwise).
* \returns True if there was actually a hit (i.e. not owner, and target is
* not immune), false otherwise.
*/
bool Plunger::hit(AbstractKart *kart, PhysicalObject *obj)
{
if(isOwnerImmunity(kart)) return false;
// pulling back makes no sense in battle mode, since this mode is not a race.
// so in battle mode, always hide view
if( m_reverse_mode || race_manager->isBattleMode() )
{
if(kart)
{
kart->blockViewWithPlunger();
if (kart->getController()->isPlayerController())
SFXManager::get()->quickSound("plunger");
}
m_keep_alive = 0;
// Make this object invisible.
getNode()->setVisible(false);
World::getWorld()->getPhysics()->removeBody(getBody());
}
else
{
m_keep_alive = m_owner->getKartProperties()->getRubberBandDuration() *
m_owner->getPlayerDifficulty()->getRubberBandDuration();
// Make this object invisible by placing it faaar down. Not that if this
// objects is simply removed from the scene graph, it might be auto-deleted
// because the ref count reaches zero.
scene::ISceneNode *node = getNode();
if(node)
{
node->setVisible(false);
}
World::getWorld()->getPhysics()->removeBody(getBody());
if(kart)
{
m_rubber_band->hit(kart);
return false;
}
else if(obj)
{
Vec3 pos(obj->getBody()->getWorldTransform().getOrigin());
m_rubber_band->hit(NULL, &pos);
}
else
{
m_rubber_band->hit(NULL, &(getXYZ()));
}
}
// Rubber band attached.
return false;
} // hit
/** Updates the graphics model. Mainly set the graphical position to be the
* same as the physics position, but uses offsets to position and rotation
* for special gfx effects (e.g. skidding will turn the karts more).
* \param offset_xyz Offset to be added to the position.
* \param rotation Additional rotation.
*/
void Moveable::updateGraphics(float dt, const Vec3& offset_xyz,
const btQuaternion& rotation)
{
Vec3 xyz=getXYZ()+offset_xyz;
m_node->setPosition(xyz.toIrrVector());
btQuaternion r_all = getRotation()*rotation;
if(btFuzzyZero(r_all.getX()) && btFuzzyZero(r_all.getY()-0.70710677f) &&
btFuzzyZero(r_all.getZ()) && btFuzzyZero(r_all.getW()-0.70710677f) )
r_all.setX(0.000001f);
Vec3 hpr;
hpr.setHPR(r_all);
m_node->setRotation(hpr.toIrrHPR());
} // updateGraphics
/** Updates the height of the rubber ball, and if necessary also adjusts the
* maximum height of the ball depending on distance from the target. The
* height is decreased when the ball is getting closer to the target so it
* hops faster and faster. This function modifies m_current_max_height.
* \return Returns the new height of the ball.
*/
float RubberBall::updateHeight()
{
// When the ball hits the floor, we adjust maximum height and
// interval so that the ball bounces faster when it is getting
// closer to the target.
if(m_height_timer>m_interval)
{
m_height_timer -= m_interval;
if(m_ping_sfx->getStatus()!=SFXManager::SFX_PLAYING)
{
m_ping_sfx->position(getXYZ());
m_ping_sfx->play();
}
if(m_fast_ping)
{
// Some experimental formulas
m_current_max_height = 0.5f*sqrt(m_distance_to_target);
// If the ball just missed the target, m_distance_to_target
// can be huge (close to track length) due to the order in
// which a lost target is detected. Avoid this by clamping
// m_current_max_height.
if(m_current_max_height>m_max_height)
m_current_max_height = m_max_height;
m_interval = m_current_max_height / 10.0f;
// Avoid too small hops and esp. a division by zero
if(m_interval<0.01f)
m_interval = 0.1f;
}
else
{
// Reset the values in case that the ball was already trying
// to get closer to the target, and then the target disappears
// (e.g. is eliminated or finishes the race).
m_interval = m_st_interval;
m_current_max_height = m_max_height;
}
} // if m_height_timer > m_interval
// Determine the height of the ball
// ================================
// Consider f(x) = s * x*(x-m_intervall), which is a parabolic function
// with f(0) = 0, f(m_intervall)=0. We then scale this function to
// fulfill: f(m_intervall/2) = max_height, or:
// f(m_interval/2) = s*(-m_interval^2)/4 = max_height
// --> s = 4*max_height / -m_interval^2
float s = 4.0f * m_current_max_height / (-m_interval*m_interval);
return m_height_timer * (m_height_timer-m_interval) * s;
} // updateHeight
/** Checks if the line from the previous ball position to the new position
* hits something, which indicates that the ball is tunneling through. If
* this happens, the ball position is adjusted so that it is just before
* the hit point. If tunneling happens four frames in a row the ball is
* considered stuck and explodes (e.g. the ball might try to tunnel through
* a wall to get to a 'close' target. In this case the ball would not
* move much anymore and be stuck).
* \return True if the ball tunneled often enough to be removed.
*/
bool RubberBall::checkTunneling()
{
const TriangleMesh &tm = World::getWorld()->getTrack()->getTriangleMesh();
Vec3 hit_point;
const Material *material;
tm.castRay(m_previous_xyz, getXYZ(), &hit_point, &material);
if(material)
{
// If there are three consecutive tunnelling
m_tunnel_count++;
if(m_tunnel_count > 3)
{
#ifdef PRINT_BALL_REMOVE_INFO
Log::debug("RubberBall",
"Ball %d nearly tunneled at %f %f %f -> %f %f %f",
m_id, m_previous_xyz.getX(),m_previous_xyz.getY(),
m_previous_xyz.getZ(),
getXYZ().getX(),getXYZ().getY(),getXYZ().getZ());
#endif
hit(NULL);
return true;
}
// In case of a hit, move the hit point towards the
// previous point by the radius of the ball --> this
// point will just allow the ball to avoid tunneling.
Vec3 diff = m_previous_xyz - hit_point;
hit_point += diff * (1.1f*m_extend.getY()/diff.length());
setXYZ(hit_point);
return false;
}
else
m_tunnel_count = 0;
return false;
} // checkTunneling
double HMC5883L::getHeadingXY()
{
int16_t raw_data[3];
getXYZ(raw_data);
double heading = atan2(static_cast<double>(raw_data[2]), static_cast<double>(raw_data[0])); // heading = arctan(Y/X)
// TODO: declenation angle compensation
if(heading < 0.0) // fix sign
heading += PI2;
if(heading > PI2) // fix overflow
heading -= PI2;
return heading;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr CloudGrabberImpl::getXYZ() {
if (m_ss == IDLE) {
printf("start streaming just for this point cloud\n");
startXYZ();
pcl::PointCloud<pcl::PointXYZ>::Ptr out = getXYZ();
stop();
return out;
}
else if (m_ss == XYZ) {
m_rs = PENDING;
printf("waiting...\n");
while (m_rs == PENDING) sleep(.01);
printf("ok\n");
return m_xyz;
}
else PRINT_AND_THROW("asked for xyz but currently streaming xyzrgb");
}
void ADXL335::getAcceleration(float *ax,float *ay,float *az)
{
int x,y,z;
float xvoltage,yvoltage,zvoltage;
getXYZ(&x,&y,&z);
xvoltage = (float)x*ADC_REF/ADC_AMPLITUDE;
yvoltage = (float)y*ADC_REF/ADC_AMPLITUDE;
zvoltage = (float)z*ADC_REF/ADC_AMPLITUDE;
Serial.println("voltage:");
Serial.println(xvoltage);
Serial.println(yvoltage);
Serial.println(zvoltage);
*ax = (xvoltage - ZERO_X)/SENSITIVITY;
*ay = (yvoltage - ZERO_Y)/SENSITIVITY;
*az = (zvoltage - ZERO_Z)/SENSITIVITY;
}
/** @brief calculate the real target of the weapon and the angle it should fire at
* @todo: doesn't take into account the drop or air resistance
*/
Quaternion Unit::getBallisticAngle(const Vector3& a_position)
{
// get the intended target
Vector3 pointer_position = Controller->getPointerPos();
// get the vector pointing at the inteded target
Vector3 pointer_direction = pointer_position - a_position;
// get the flattened vector
pointer_direction.y = 0;
// get the real direction confined to the torso angle
Real distance = pointer_direction.length();
pointer_direction = getDirection() * distance;
// get the point the vector is pointing at from the perspective of the weapon
pointer_position = pointer_direction + a_position;
// we need to apply the offset so that the angles converge from the weapons mounted on the sides
Vector3 weapon_offset = getXYZ() - a_position;
// the angle must be limited otherwise you could fire sideways - this limits it to 5 degrees
Real limit = distance / 11.4300523;
if (limit < weapon_offset.length()) {
weapon_offset.normalise();
weapon_offset *= limit;
}
pointer_position += weapon_offset;
// check if the real target is within the arena
Game::Arena->isOutOfArena(pointer_position);
// get the height at this point
Real height = Game::Arena->getHeight(pointer_position.x, pointer_position.z);
// adjust by the target mode
if (Controller->ControlBlock.target_high) {
pointer_position.y = height + target_high_offset;
} else if (Controller->ControlBlock.target_air) {
pointer_position.y = height + target_air_offset;
} else {
pointer_position.y = height + target_low_offset;
}
// the real direction to the real target
pointer_direction = pointer_position - a_position;
// return the quaternion for the projectile
return Vector3::UNIT_Z.getRotationTo(pointer_direction);
}
void ADXL335::getPitchAndRoll(float *roll, float *pitch)
{
int x,y,z;
float xvoltage,yvoltage,zvoltage, ax, ay, az;
getXYZ(&x,&y,&z);
xvoltage = (float)x*ADC_REF/ADC_AMPLITUDE;
yvoltage = (float)y*ADC_REF/ADC_AMPLITUDE;
zvoltage = (float)z*ADC_REF/ADC_AMPLITUDE;
//Serial.println("voltage:");
//Serial.println(xvoltage);
//Serial.println(yvoltage);
//Serial.println(zvoltage);
ax = (xvoltage - ZERO_X)/SENSITIVITY;
ay = (yvoltage - ZERO_Y)/SENSITIVITY;
az = (zvoltage - ZERO_Z)/SENSITIVITYx+1;
*pitch=atan2(ax,az)*180/3.141592;
*roll=atan2(ay,az)*180/3.141592;
}
/** Sets up the control points for the interpolation. The parameter contains
* the coordinates of the first control points (i.e. a control point that
* influences the direction the ball is flying only, not the actual
* coordinates - see details about Catmull-Rom splines). This function will
* then set the 2nd control point to be the current coordinates of the ball,
* and find two more appropriate control points for a smooth movement.
* \param xyz Coordinates of the first control points.
*/
void RubberBall::initializeControlPoints(const Vec3 &xyz)
{
m_control_points[0] = xyz;
m_control_points[1] = getXYZ();
m_last_aimed_graph_node = getSuccessorToHitTarget(getCurrentGraphNode());
// This call defined m_control_points[3], but also sets a new
// m_last_aimed_graph_node, which is further away from the current point,
// which avoids the problem that the ball might go too quickly to the
// left or right when firing the ball off track.
getNextControlPoint();
m_control_points[2] =
QuadGraph::get()->getQuadOfNode(m_last_aimed_graph_node).getCenter();
// This updates m_last_aimed_graph_node, and sets m_control_points[3]
getNextControlPoint();
m_length_cp_1_2 = (m_control_points[2]-m_control_points[1]).length();
m_t = 0;
m_t_increase = m_speed/m_length_cp_1_2;
} // initializeControlPoints
static void getYml(GLenum face, size_t width, size_t height,
float *Y00,
float *Y1minus1, float *Y10, float *Y11,
float *Y2minus2, float *Y2minus1, float *Y20, float *Y21, float *Y22)
{
for (unsigned i = 0; i < width; i++)
{
for (unsigned j = 0; j < height; j++)
{
float x, y, z;
float fi = float(i), fj = float(j);
fi /= width, fj /= height;
fi = 2 * fi - 1, fj = 2 * fj - 1;
getXYZ(face, fi, fj, x, y, z);
// constant part of Ylm
float c00 = 0.282095f;
float c1minus1 = 0.488603f;
float c10 = 0.488603f;
float c11 = 0.488603f;
float c2minus2 = 1.092548f;
float c2minus1 = 1.092548f;
float c21 = 1.092548f;
float c20 = 0.315392f;
float c22 = 0.546274f;
size_t idx = i * height + j;
Y00[idx] = c00;
Y1minus1[idx] = c1minus1 * y;
Y10[idx] = c10 * z;
Y11[idx] = c11 * x;
Y2minus2[idx] = c2minus2 * x * y;
Y2minus1[idx] = c2minus1 * y * z;
Y21[idx] = c21 * x * z;
Y20[idx] = c20 * (3 * z * z - 1);
Y22[idx] = c22 * (x * x - y * y);
}
}
}
/// Loads and normalizes all PNGs into a tensor memory block \p resultT in the
/// NCHW 3x224x224 format.
static void loadImagesAndPreprocess(const std::vector<std::string> &filenames,
float *&resultT, size_t *resultDims) {
assert(filenames.size() > 0 &&
"There must be at least one filename in filenames");
std::pair<float, float> range = std::make_pair(0., 256.0);
unsigned numImages = filenames.size();
// N x C x H x W
resultDims[0] = numImages;
resultDims[1] = 3;
resultDims[2] = DEFAULT_HEIGHT;
resultDims[3] = DEFAULT_WIDTH;
size_t resultSizeInBytes =
numImages * 3 * DEFAULT_HEIGHT * DEFAULT_WIDTH * sizeof(float);
resultT = static_cast<float *>(malloc(resultSizeInBytes));
// We iterate over all the png files, reading them all into our result tensor
// for processing
for (unsigned n = 0; n < numImages; n++) {
float *imageT{nullptr};
size_t dims[3];
bool loadSuccess = !readPngImage(filenames[n].c_str(), range, imageT, dims);
assert(loadSuccess && "Error reading input image.");
assert((dims[0] == DEFAULT_HEIGHT && dims[1] == DEFAULT_WIDTH) &&
"All images must have the same Height and Width");
// Convert to BGR, as this is what NN is expecting.
for (unsigned z = 0; z < 3; z++) {
for (unsigned y = 0; y < dims[1]; y++) {
for (unsigned x = 0; x < dims[0]; x++) {
resultT[getXYZW(resultDims, n, 2 - z, x, y)] =
imageT[getXYZ(dims, x, y, z)];
}
}
}
}
printf("Loaded images size in bytes is: %lu\n", resultSizeInBytes);
}
/** Moves the rubber ball in a straight line towards the target. This is used
* once the rubber ball is close to its target. It restricts the angle by
* which the rubber ball can change its direction per frame.
* \param next_xyz The position the ball should move to.
* \param dt Time step size.
*/
void RubberBall::moveTowardsTarget(Vec3 *next_xyz, float dt)
{
// If the rubber ball is already close to a target, i.e. aiming
// at it directly, stop interpolating, instead fly straight
// towards it.
Vec3 diff = m_target->getXYZ()-getXYZ();
// Avoid potential division by zero
if(diff.length2()==0)
*next_xyz = getXYZ();
else
*next_xyz = getXYZ() + (dt*m_speed/diff.length())*diff;
Vec3 old_vec = getXYZ()-m_previous_xyz;
Vec3 new_vec = *next_xyz - getXYZ();
float angle = atan2(new_vec.getZ(), new_vec.getX())
- atan2(old_vec.getZ(), old_vec.getX());
// Adjust angle to be between -180 and 180 degrees
if(angle < -M_PI)
angle += 2*M_PI;
else if(angle > M_PI)
angle -= 2*M_PI;
// If the angle is too large, adjust next xyz
if(fabsf(angle)>m_st_target_max_angle*dt)
{
core::vector2df old_2d(old_vec.getX(), old_vec.getZ());
if(old_2d.getLengthSQ()==0.0f) old_2d.Y = 1.0f;
old_2d.normalize();
old_2d.rotateBy( RAD_TO_DEGREE * dt
* (angle > 0 ? m_st_target_max_angle
: -m_st_target_max_angle));
next_xyz->setX(getXYZ().getX() + old_2d.X*dt*m_speed);
next_xyz->setZ(getXYZ().getZ() + old_2d.Y*dt*m_speed);
} // if fabsf(angle) > m_st_target_angle_max*dt
assert(!isnan((*next_xyz)[0]));
assert(!isnan((*next_xyz)[1]));
assert(!isnan((*next_xyz)[2]));
} // moveTowardsTarget
void SpecificWorker::getImage(ColorSeq& color, DepthSeq& depth, PointSeq& points, RoboCompJointMotor::MotorStateMap &hState, RoboCompDifferentialRobot::TBaseState& bState)
{
getDepth(depth,hState,bState);
getRGB(color,hState,bState);
getXYZ(points,hState,bState);
}
void IsoSurfaceThread::run()
{
int numThreads = GLFunctions::idealThreadCount;
int chunkSize = m_scalarField->size() / numThreads;
int begin = m_id * chunkSize;
int end = m_id * chunkSize + chunkSize;
if ( m_id == numThreads - 1 )
{
end = m_scalarField->size() - ( ( m_nX + 1 ) * ( m_nY + 1 ) );
}
int x;
int y;
int z;
// Generate isosurface.
for ( int k = begin; k < end; ++k )
{
getXYZ( k, x, y, z );
if ( x == m_nX ) continue;
if ( y == m_nY ) continue;
// Calculate table lookup index from those
// vertices which are below the isolevel.
unsigned int tableIndex = 0;
if ( m_scalarField->at( z * m_nPointsInSlice + y * m_nPointsInXDirection + x ) < m_isoLevel )
tableIndex |= 1;
if ( m_scalarField->at( z * m_nPointsInSlice + ( y + 1 ) * m_nPointsInXDirection + x ) < m_isoLevel )
tableIndex |= 2;
if ( m_scalarField->at( z * m_nPointsInSlice + ( y + 1 ) * m_nPointsInXDirection + ( x + 1 ) ) < m_isoLevel )
tableIndex |= 4;
if ( m_scalarField->at( z * m_nPointsInSlice + y * m_nPointsInXDirection + ( x + 1 ) ) < m_isoLevel )
tableIndex |= 8;
if ( m_scalarField->at( ( z + 1 ) * m_nPointsInSlice + y * m_nPointsInXDirection + x ) < m_isoLevel )
tableIndex |= 16;
if ( m_scalarField->at( ( z + 1 ) * m_nPointsInSlice + ( y + 1 ) * m_nPointsInXDirection + x ) < m_isoLevel )
tableIndex |= 32;
if ( m_scalarField->at( ( z + 1 ) * m_nPointsInSlice + ( y + 1 ) * m_nPointsInXDirection + ( x + 1 ) ) < m_isoLevel )
tableIndex |= 64;
if ( m_scalarField->at( ( z + 1 ) * m_nPointsInSlice + y * m_nPointsInXDirection + ( x + 1 ) ) < m_isoLevel )
tableIndex |= 128;
// Now create a triangulation of the isosurface in this
// cell.
if ( edgeTable[ tableIndex ] != 0 )
{
if ( edgeTable[ tableIndex ] & 8 )
{
POINT3DID pt = calculateIntersection( x, y, z, 3 );
unsigned int id = getEdgeID( x, y, z, 3 );
QMutexLocker locker( m_mutex );
m_i2pt3idVertices->insert( id, pt );
locker.unlock();
}
if ( edgeTable[ tableIndex ] & 1 )
{
POINT3DID pt = calculateIntersection( x, y, z, 0 );
unsigned int id = getEdgeID( x, y, z, 0 );
QMutexLocker locker( m_mutex );
m_i2pt3idVertices->insert( id, pt );
locker.unlock();
}
if ( edgeTable[ tableIndex ] & 256 )
{
POINT3DID pt = calculateIntersection( x, y, z, 8 );
unsigned int id = getEdgeID( x, y, z, 8 );
QMutexLocker locker( m_mutex );
m_i2pt3idVertices->insert( id, pt );
locker.unlock();
}
if ( x == m_nX - 1 )
{
if ( edgeTable[ tableIndex ] & 4 )
{
POINT3DID pt = calculateIntersection( x, y, z, 2 );
unsigned int id = getEdgeID( x, y, z, 2 );
QMutexLocker locker( m_mutex );
m_i2pt3idVertices->insert( id, pt );
locker.unlock();
}
if ( edgeTable[ tableIndex ] & 2048 )
{
POINT3DID pt = calculateIntersection( x, y, z, 11 );
unsigned int id = getEdgeID( x, y, z, 11 );
QMutexLocker locker( m_mutex );
m_i2pt3idVertices->insert( id, pt );
locker.unlock();
}
}
if ( y == m_nY - 1 )
{
if ( edgeTable[ tableIndex ] & 2 )
{
POINT3DID pt = calculateIntersection( x, y, z, 1 );
unsigned int id = getEdgeID( x, y, z, 1 );
QMutexLocker locker( m_mutex );
m_i2pt3idVertices->insert( id, pt );
locker.unlock();
}
if ( edgeTable[ tableIndex ] & 512 )
{
POINT3DID pt = calculateIntersection( x, y, z, 9 );
unsigned int id = getEdgeID( x, y, z, 9 );
QMutexLocker locker( m_mutex );
m_i2pt3idVertices->insert( id, pt );
locker.unlock();
}
}
if ( z == m_nZ - 1 )
{
if ( edgeTable[ tableIndex ] & 16 )
{
POINT3DID pt = calculateIntersection( x, y, z, 4 );
unsigned int id = getEdgeID( x, y, z, 4 );
QMutexLocker locker( m_mutex );
m_i2pt3idVertices->insert( id, pt );
locker.unlock();
}
if ( edgeTable[ tableIndex ] & 128 )
{
POINT3DID pt = calculateIntersection( x, y, z, 7 );
unsigned int id = getEdgeID( x, y, z, 7 );
QMutexLocker locker( m_mutex );
m_i2pt3idVertices->insert( id, pt );
locker.unlock();
}
}
if ( ( x == m_nX - 1 ) && ( y == m_nY - 1 ) )
if ( edgeTable[ tableIndex ] & 1024 )
{
POINT3DID pt = calculateIntersection( x, y, z, 10 );
unsigned int id = getEdgeID( x, y, z, 10 );
QMutexLocker locker( m_mutex );
m_i2pt3idVertices->insert( id, pt );
locker.unlock();
}
if ( ( x == m_nX - 1 ) && ( z == m_nZ - 1 ) )
if ( edgeTable[ tableIndex ] & 64 )
{
POINT3DID pt = calculateIntersection( x, y, z, 6 );
unsigned int id = getEdgeID( x, y, z, 6 );
QMutexLocker locker( m_mutex );
m_i2pt3idVertices->insert( id, pt );
locker.unlock();
}
if ( ( y == m_nY - 1 ) && ( z == m_nZ - 1 ) )
if ( edgeTable[ tableIndex ] & 32 )
{
POINT3DID pt = calculateIntersection( x, y, z, 5 );
unsigned int id = getEdgeID( x, y, z, 5 );
QMutexLocker locker( m_mutex );
m_i2pt3idVertices->insert( id, pt );
locker.unlock();
}
for ( int i = 0; triTable[ tableIndex ][ i ] != -1; i += 3 )
{
TRIANGLE triangle;
unsigned int pointID0, pointID1, pointID2;
pointID0 = getEdgeID( x, y, z, triTable[ tableIndex ][ i ] );
pointID1 = getEdgeID( x, y, z, triTable[ tableIndex ][ i + 1 ] );
pointID2 = getEdgeID( x, y, z, triTable[ tableIndex ][ i + 2 ] );
triangle.pointID[ 0 ] = pointID0;
triangle.pointID[ 1 ] = pointID1;
triangle.pointID[ 2 ] = pointID2;
QMutexLocker locker( m_mutex );
m_trivecTriangles->push_back( triangle );
locker.unlock();
}
}
}
}
/** Updates the bowling ball ineach frame. If this function returns true, the
* object will be removed by the projectile manager.
* \param dt Time step size.
* \returns True of this object should be removed.
*/
bool Bowling::updateAndDelete(float dt)
{
bool can_be_deleted = Flyable::updateAndDelete(dt);
if(can_be_deleted)
return true;
const AbstractKart *kart=0;
Vec3 direction;
float minDistance;
getClosestKart(&kart, &minDistance, &direction);
if(kart && minDistance<m_st_max_distance_squared) // move bowling towards kart
{
// limit angle, so that the bowling ball does not turn
// around to hit a kart behind
if(fabs(m_body->getLinearVelocity().angle(direction)) < 1.3)
{
direction*=1/direction.length()*m_st_force_to_target;
m_body->applyCentralForce(direction);
}
}
// Bowling balls lose energy (e.g. when hitting the track), so increase
// the speed if the ball is too slow, but only if it's not too high (if
// the ball is too high, it is 'pushed down', which can reduce the
// speed, which causes the speed to increase, which in turn causes
// the ball to fly higher and higher.
//btTransform trans = getTrans();
float hat = getXYZ().getY()-getHoT();
if(hat-0.5f*m_extend.getY()<0.01f)
{
const Material *material = getMaterial();
if(!material || material->isDriveReset())
{
hit(NULL);
return true;
}
}
btVector3 v = m_body->getLinearVelocity();
float vlen = v.length2();
if (hat<= m_max_height)
{
if(vlen<0.8*m_speed*m_speed)
{ // bowling lost energy (less than 80%), i.e. it's too slow - speed it up:
if(vlen==0.0f) {
v = btVector3(.5f, .0, 0.5f); // avoid 0 div.
}
// m_body->setLinearVelocity(v*(m_speed/sqrt(vlen)));
} // vlen < 0.8*m_speed*m_speed
} // hat< m_max_height
if(vlen<0.1)
{
hit(NULL);
return true;
}
if (m_roll_sfx->getStatus()==SFXBase::SFX_PLAYING)
m_roll_sfx->setPosition(getXYZ());
return false;
} // updateAndDelete