/** 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