Value* ConstDoubleValue::negConstant(Procedure& proc) const
{
return proc.add<ConstDoubleValue>(origin(), -m_value);
}
ray ray::operator-() const {
return ray(origin(), -direction());
}
static Vec2 calculateItemPositionWithAnchor(Widget* item, const Vec2& itemAnchorPoint)
{
Vec2 origin(item->getLeftBoundary(), item->getBottomBoundary());
Size size = item->getContentSize();
return origin + Vec2(size.width * itemAnchorPoint.x, size.height * itemAnchorPoint.y);
}
StencilTable const *
StencilTableFactory::AppendLocalPointStencilTable(
TopologyRefiner const &refiner,
StencilTable const * baseStencilTable,
StencilTable const * localPointStencilTable,
bool factorize) {
// factorize and append.
if (baseStencilTable == NULL ||
localPointStencilTable == NULL ||
localPointStencilTable->GetNumStencils() == 0) return NULL;
// baseStencilTable can be built with or without singular stencils
// (single weight of 1.0f) as place-holders for coarse mesh vertices.
int controlVertsIndexOffset = 0;
int nBaseStencils = baseStencilTable->GetNumStencils();
int nBaseStencilsElements = (int)baseStencilTable->_indices.size();
{
int nverts = refiner.GetNumVerticesTotal();
if (nBaseStencils == nverts) {
// the table contain stencils for the control vertices
//
// <----------------- nverts ------------------>
//
// +---------------+----------------------------+-----------------+
// | control verts | refined verts : (max lv) | local points |
// +---------------+----------------------------+-----------------+
// | base stencil table | LP stencils |
// +--------------------------------------------+-----------------+
// ^ /
// \_________________________/
//
//
controlVertsIndexOffset = 0;
} else if (nBaseStencils == (nverts -refiner.GetLevel(0).GetNumVertices())) {
// the table does not contain stencils for the control vertices
//
// <----------------- nverts ------------------>
// <------ nBaseStencils ------->
// +---------------+----------------------------+-----------------+
// | control verts | refined verts : (max lv) | local points |
// +---------------+----------------------------+-----------------+
// | base stencil table | LP stencils |
// +----------------------------+-----------------+
// ^ /
// \_________________/
// <-------------->
// controlVertsIndexOffset
//
controlVertsIndexOffset = refiner.GetLevel(0).GetNumVertices();
} else {
// these are not the stencils you are looking for.
assert(0);
return NULL;
}
}
// copy all local points stencils to proto stencils, and factorize if needed.
int nLocalPointStencils = localPointStencilTable->GetNumStencils();
int nLocalPointStencilsElements = 0;
internal::StencilBuilder builder(refiner.GetLevel(0).GetNumVertices(),
/*genControlVerts*/ false,
/*compactWeights*/ factorize);
internal::StencilBuilder::Index origin(&builder, 0);
internal::StencilBuilder::Index dst = origin;
internal::StencilBuilder::Index srcIdx = origin;
for (int i = 0 ; i < nLocalPointStencils; ++i) {
Stencil src = localPointStencilTable->GetStencil(i);
dst = origin[i];
for (int j = 0; j < src.GetSize(); ++j) {
Index index = src.GetVertexIndices()[j];
float weight = src.GetWeights()[j];
if (isWeightZero(weight)) continue;
if (factorize) {
dst.AddWithWeight(
// subtracting controlVertsIndex if the baseStencil doesn't
// include control vertices (see above diagram)
// since currently local point stencils are created with
// absolute indices including control (level=0) vertices.
baseStencilTable->GetStencil(index - controlVertsIndexOffset),
weight);
} else {
srcIdx = origin[index + controlVertsIndexOffset];
dst.AddWithWeight(srcIdx, weight);
}
}
nLocalPointStencilsElements += builder.GetNumVertsInStencil(i);
}
// create new stencil table
StencilTable * result = new StencilTable;
result->_numControlVertices = refiner.GetLevel(0).GetNumVertices();
result->resize(nBaseStencils + nLocalPointStencils,
nBaseStencilsElements + nLocalPointStencilsElements);
int* sizes = &result->_sizes[0];
Index * indices = &result->_indices[0];
float * weights = &result->_weights[0];
// put base stencils first
memcpy(sizes, &baseStencilTable->_sizes[0],
nBaseStencils*sizeof(int));
memcpy(indices, &baseStencilTable->_indices[0],
nBaseStencilsElements*sizeof(Index));
memcpy(weights, &baseStencilTable->_weights[0],
nBaseStencilsElements*sizeof(float));
sizes += nBaseStencils;
indices += nBaseStencilsElements;
weights += nBaseStencilsElements;
// endcap stencils second
for (int i = 0 ; i < nLocalPointStencils; ++i) {
int size = builder.GetNumVertsInStencil(i);
int idx = builder.GetStencilOffsets()[i];
for (int j = 0; j < size; ++j) {
*indices++ = builder.GetStencilSources()[idx+j];
*weights++ = builder.GetStencilWeights()[idx+j];
}
*sizes++ = size;
}
// have to re-generate offsets from scratch
result->generateOffsets();
return result;
}
DeltaGlider::DeltaGlider(double initial_x_position, double initial_y_position, double initial_x_velocity, double initial_y_velocity, double initial_theta, double initial_omega, double initial_main_propellant, double initial_rcs_propellant, sf::Texture vessel_textures, sf::Sprite flag_sprite, std::string ivessel_name, std::string panel_path, std::string controls_font, Propagator_type propagator)
{ Talkback("Constructing Delta Glider");
// we write to the console for feedback while debugging
NewtonianState.Current_state = Flight;
Propagator = propagator;
Start_MFD(surface_mfd, upper_right, sf::Vector2f(20, 60));
Flag_sprite = flag_sprite;
// set our flag affilation here
// this looks wrongish, I think this needs to be copied by value, definitely
// not reference, since the same flag may need to be drawn several times per
// frame
Flag_sprite.setScale(0.1,0.1);
// scales it back by 1/10 along each axis, since the original was too big
// this is baddish
Object_name = ivessel_name;
Vessel_class = "Delta Glider";
// this should rather be frozen so it cant be changed in sim, just for
// safety/not chaos
Acceleration.Set_values(0, 0);
// just an call to be safe
NewtonianState.FlightState.Position.Set_values(initial_x_position, initial_y_position);
NewtonianState.FlightState.Velocity.Set_values(initial_x_velocity, initial_y_velocity);
this->NewtonianState.Rotation.Theta = initial_theta;
this->NewtonianState.Rotation.Omega = initial_omega;
this->NewtonianState.Rotation.Alpha = 0;
// we take what we were passed and set our properties to it
// important that this gets set explicitly, without it the compiler just
// sets it to random values for some godawful reason
VectorVictor::Vector2 origin(0,0);
Hull_component = new Hull(16000, 0.4, 17.5, origin);
Object_components.insert(Object_components.end(), Hull_component);
// we construct the hull component and insert it into the general component
// list. The vessel can access it by either the Hull component reference
// that it stores internally, or through its general version in the parts
// list
// the pointer passin crap didnt work ^^
// ^ not sure exactly what that was referring to
Main_fuel = new Resource_Tank(initial_main_propellant, 800, 0.6, 0.8, 2.2, origin);
Fuel_tanks.insert(Fuel_tanks.end(), Main_fuel);
Object_components.insert(Object_components.end(), Main_fuel->Get_vessel_component_pointer());
// construct the main fuel tank and insert it into both the fuel tank list
// and the general parts list.
// Come to think of it, why exactly is there a separate list for fuel tanks?
// they arent even accessed that way, not yet anyways
Init_thruster(false, 4000, 50000, 290, 0, -6.2000, 0, -1, 0.4, 0.49, Main_fuel, main_engines);
// construct the main engine thrsuter, insert it into the thruster vector
// and the general components vector
// this is at least necessary, since the main thruster vector is used for
// throttling back all engines each frame unless the throttle up is doing
// the opposite
VectorVictor::Vector2 rcs_pos(0, 2);
RCS_fuel = new Resource_Tank(initial_rcs_propellant, 500, 0.6, 0.64, 1,rcs_pos);
Fuel_tanks.insert(Fuel_tanks.end(), RCS_fuel);
Object_components.insert(Object_components.end(), RCS_fuel->Get_vessel_component_pointer());
Init_thruster(true, 470, 49000, 61, 0, 6.2000, -1, 0, 0.10, 0.12, RCS_fuel, rotate_clockwise, translate_right);
Init_thruster(true, 470, 49000, 61, 0, 6.2000, 1, 0, 0.10, 0.12, RCS_fuel, rotate_counterclockwise, translate_left);
Init_thruster(true, 470, 49000, 46, 0, 6.400, 0, 1, 0.10, 0.12, RCS_fuel, translate_back);
Init_thruster(true, 470, 49000, 63, -0.20, -6.0000, -1, 0, 0.10, 0.12, RCS_fuel, rotate_counterclockwise, translate_right);
Init_thruster(true, 470, 49000, 63, -0.20, -6.0000, 1, 0, 0.10, 0.12, RCS_fuel, rotate_clockwise, translate_left);
Init_thruster(true, 470, 49000, 46, -0.2, -6.0000, 0, -1, 0.10, 0.12, RCS_fuel, translate_forward);
// and we construct all of the RCS thrusters in the same way
std::cout << Object_components.size() << " Vessel components" << std::endl;
// not sure why, but good to have the number of components on the vessel
// for debugging
RCS_Throttle_constant = 29.9000;
// we set this up so that the thruster goes from 0 to 2990 % in one second
// obviously that doesnt work, so it really represents 0 to 100% in 1/29.9 s
// (0.033 s, roughly 2 frames if fps is a healthy 60)
Main_throttle_constant = 0.25;
// significantly slower for the main engines, here they take a full four
// seconds to go from 0 - 100
Throttle_lock = false;
// Throttle lock keeps the main engines at their current throttle setting
// instead of trying to slide back to zero each frame, which is useful
// during long burns
Update_PMI();
// once all of the vessel components are in, this should be run to get the
// initial value
// we create it and assign it the texture passed by the constructor
// (less memory waste per vessel than each one loading a copy of the texture
// for every instance
Talkback("Setting up texture stuff");
Vessel_tex = new sf::Texture();
*Vessel_tex = vessel_textures;
Talkback("Copied texture");
// not sure why we need to copy the reference to this...
Object_sprite = new sf::Sprite();
Talkback("Init the sprite");
Object_sprite->setTexture(*Vessel_tex);
Talkback("Finished textures stuff");
// this appears to be redundant, and a good candidate to remove
text_colour = new sf::Color(194, 38, 15);
// nice red colour for all of the vessel displays
vessel_id.Init_object(controls_font, sf::Vector2f(20, 380), "-", *text_colour, 16, false);
main_fuel_level.Init_object(controls_font, sf::Vector2f(20, 420), "-", *text_colour, 14, false);
rcs_fuel_level.Init_object(controls_font, sf::Vector2f(20, 440), "-", *text_colour, 14, false);
omega_value.Init_object(controls_font, sf::Vector2f(20, 460), "-", *text_colour, 14, false);
theta_value.Init_object(controls_font, sf::Vector2f(20, 480), "-", *text_colour, 14, false);
position_values.Init_object(controls_font, sf::Vector2f(20, 500), "-", *text_colour, 14, false);
velocity_values.Init_object(controls_font, sf::Vector2f(20, 520), "-", *text_colour, 14, false);
vessel_display.Init_object(sf::Vector2f(820, 460), sf::Color(255, 255, 255, 168), false, *Vessel_tex, true);
vessel_display.sprite.setScale(sf::Vector2f(0.5f, 0.5f));
// Ill get around to it later
// the pretty semi-transparent image of the ship that appears at bottom
// right in the screen. Really just eye candy for now, but I have plans for
// it later on. Its vessel implementation-specific anywayss
display_panel.Init_object(sf::Vector2f(5, 360), sf::Color(255, 255, 255), false, panel_path);
// Ill get around to it later
// display_panel = new sf::Sprite();
// display_panel->setTexture(*panel_texture1);
// display_panel->setPosition(sf::Vector2f(5, 360));
// the metal looking thing at bottom right. Only there because the onscreen
// displays proved hard to read against the background
pix_length = Hull_component->Length*10;
// get the length of the vessels long axis in meters
// the multiple of ten thing is fine, but the current window size was
// accidentally made 10x too large, so vessels appeared 10x larger than
// they really were
// not terribly important, since this will change when the window view
// has multiple scales to be a lot like the one for planets, but it is
// important to remember that the base view has 1m = 10 pixels, for reasons
// of easiest scaling
if(Vessel_tex->getSize().y != pix_length)
{ Object_sprite->setScale((pix_length/(Vessel_tex->getSize().y)),(pix_length/(Vessel_tex->getSize().y)));
// rescale the axes of the texture to match pix_length in the y and the
// appropriate scale for the x dimension
// not sure why they both use y, but I think this was distorted when
// it used x & y.
// This area needs to be looked over again
}
Object_sprite->setOrigin((Vessel_tex->getSize().x/2), (Vessel_tex->getSize().y/2));
// ok the sprite definitely gets centered there...
Object_sprite->setRotation(this->NewtonianState.Rotation.Theta);
// and we center the origin and rotate it appropriately
Talkback("...Constructed Delta Glider");
// feed back that the constructor is now finished to the console
}
/* Initialize the Albers projection
-------------------------------*/
long alberforint(
double r_maj, /* major axis */
double r_min, /* minor axis */
double lat1, /* first standard parallel */
double lat2, /* second standard parallel */
double lon0, /* center longitude */
double lat0, /* center lattitude */
double false_east, /* x offset in meters */
double false_north) /* y offset in meters */
{
double sin_po,cos_po; /* sin and cos values */
double con; /* temporary variable */
double es,temp; /* eccentricity squared and temp var */
double ms1; /* small m 1 */
double ms2; /* small m 2 */
double qs0; /* small q 0 */
double qs1; /* small q 1 */
double qs2; /* small q 2 */
false_easting = false_east;
false_northing = false_north;
lon_center = lon0;
if (fabs(lat1 + lat2) < EPSLN)
{
p_error("Equal latitudes for St. Parallels on opposite sides of equator",
"alber-forinit");
return(31);
}
r_major = r_maj;
r_minor = r_min;
temp = r_minor / r_major;
es = 1.0 - SQUARE(temp);
e3 = sqrt(es);
gctp_sincos(lat1, &sin_po, &cos_po);
con = sin_po;
ms1 = msfnz(e3,sin_po,cos_po);
qs1 = qsfnz(e3,sin_po );
gctp_sincos(lat2,&sin_po,&cos_po);
ms2 = msfnz(e3,sin_po,cos_po);
qs2 = qsfnz(e3,sin_po );
gctp_sincos(lat0,&sin_po,&cos_po);
qs0 = qsfnz(e3,sin_po );
if (fabs(lat1 - lat2) > EPSLN)
ns0 = (ms1 * ms1 - ms2 *ms2)/ (qs2 - qs1);
else
ns0 = con;
c = ms1 * ms1 + ns0 * qs1;
rh = r_major * sqrt(c - ns0 * qs0)/ns0;
/* Report parameters to the user
-----------------------------*/
ptitle("ALBERS CONICAL EQUAL-AREA");
radius2(r_major, r_minor);
stanparl(lat1,lat2);
cenlonmer(lon_center);
origin(lat0);
offsetp(false_easting,false_northing);
return(OK);
}
void AdvancedPlayerController (DemoEntityManager* const scene)
{
// load the sky box
scene->CreateSkyBox();
NewtonWorld* const world = scene->GetNewton();
// add an input Manage to manage the inputs and user interaction
AdvancedPlayerInputManager* const inputManager = new AdvancedPlayerInputManager (scene);
// create a character controller manager
AdvancePlayerControllerManager* const playerManager = new AdvancePlayerControllerManager (world);
// create a scene for player controller to work around
TriggerManager* const triggerManager = new TriggerManager(world, playerManager);
// create the background scene
LoadPlayGroundScene(scene, triggerManager);
// add main player
dMatrix location (dGetIdentityMatrix());
location.m_posit.m_x = 45.0f;
location.m_posit.m_y = 5.0f;
location.m_posit.m_z = -10.0f;
location.m_posit = FindFloor (scene->GetNewton(), location.m_posit, 10.0f);
AdvancePlayerEntity* const player = new AdvancePlayerEntity (scene, playerManager, 0.5f, 1.9f, location);
// set as the player with the camera
inputManager->AddPlayer(player);
/*
//dFloat x0 = location.m_posit.m_x;
dFloat z0 = location.m_posit.m_z - 6.0f;
for (int i = 0; i < 5; i ++) {
location.m_posit.m_x += 2;
location.m_posit.m_z = z0;
for (int i = 0; i < 5; i ++) {
location.m_posit.m_z += 2;
location.m_posit.m_y = 5.0f;
location.m_posit = FindFloor (scene->GetNewton(), location.m_posit, 10.0f);
new AdvancePlayerEntity (scene, manager, 0.5f, 1.9f, location);
}
}
*/
dQuaternion rot;
dVector origin (-10.0f, 2.0f, 0.0f, 0.0f);
scene->SetCameraMatrix(rot, origin);
// configure Game Logic AI agent
// new AIAgentGameLogic (scene, playerAgent);
// AddBoxes(system, 10.0f, location, size, 3, 3, 10.0f, _SPHERE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddBoxes(system, 10.0f, location, size, 3, 3, 10.0f, _BOX_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddBoxes(system, 10.0f, location, size, 3, 3, 10.0f, _CONE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddBoxes(system, 10.0f, location, size, 3, 3, 10.0f, _CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddBoxes(system, 10.0f, location, size, 3, 3, 10.0f, _CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddBoxes(system, 10.0f, location, size, 3, 3, 10.0f, _CHAMFER_CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddBoxes(system, 10.0f, location, size, 3, 3, 10.0f, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddBoxes(system, 10.0f, location, size, 3, 3, 10.0f, _REGULAR_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// InitEyePoint (dVector (1.0f, 0.0f, 0.0f), location.m_posit);
}
/**
* This is where the localization is done... not really callback in the offline version
*/
void callback(const sensor_msgs::LaserScan &scan, tf::Transform odo_pose, tf::Transform state_pose)
{
static int counter = 0;
counter++;
static tf::TransformListener tf_listener;
double looptime = TT.Tac();
TT.Tic();
fprintf(stderr,"Lt( %.1lfms %.1lfHz seq:%d) -",looptime*1000,1.0/looptime,scan.header.seq);
if(has_sensor_offset_set == false) return;
double gx,gy,gyaw,x,y,yaw;
///Get the ground truth
gyaw = tf::getYaw(state_pose.getRotation());
gx = state_pose.getOrigin().x();
gy = state_pose.getOrigin().y();
///Get the odometry
yaw = tf::getYaw(odo_pose.getRotation());
x = odo_pose.getOrigin().x();
y = odo_pose.getOrigin().y();
mrpt::utils::CTicTac tictac;
tictac.Tic();
int N =(scan.angle_max - scan.angle_min)/scan.angle_increment; ///< number of scan lines
/////
/// Pose conversions
////
Eigen::Affine3d T = getAsAffine(x,y,yaw);
Eigen::Affine3d Tgt = getAsAffine(gx,gy,gyaw);
/**
* We are now using the information from the ground truth to initialize the filter
*/
if(isFirstLoad){
fprintf(stderr,"Initializing to (%lf, %lf, %lf)\n",gx,gy,gyaw);
///Initialize the filter with 1m^2 variance in position and 20deg in heading
ndtmcl->initializeFilter(gx, gy,gyaw,1.0, 1.0, 20.0*M_PI/180.0, 450);
Told = T;
Todo = Tgt;
}
///Compute the differential motion between two time steps
Eigen::Affine3d Tmotion = Told.inverse() * T;
Todo = Todo*Tmotion;
Told = T;
///Get the laser pose with respect to the base
float dy =offy;
float dx = offx;
float alpha = atan2(dy,dx);
float L = sqrt(dx*dx+dy*dy);
///Laser pose in base frame
float lpx = L * cos(alpha);
float lpy = L * sin(alpha);
float lpa = offa;
///Laser scan to PointCloud transformed with respect to the base
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
for (int j=0;j<N;j++){
double r = scan.ranges[j];
if(r>=scan.range_min && r<scan.range_max && r>0.3 && r<20.0){
double a = scan.angle_min + j*scan.angle_increment;
pcl::PointXYZ pt;
pt.x = r*cos(a+lpa)+lpx;
pt.y = r*sin(a+lpa)+lpy;
pt.z = 0.1+0.02 * (double)rand()/(double)RAND_MAX;
cloud->push_back(pt);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Now we have the differential motion and pointcloud -- Lets do MCL
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ndtmcl->updateAndPredict(Tmotion, *cloud); ///< does the prediction, update and resampling steps (see ndt_mcl.hpp)
Eigen::Vector3d dm = ndtmcl->getMean(); ///<Maximum aposteriori estimate of the pose
Eigen::Matrix3d cov = ndtmcl->pf.getDistributionVariances(); ///distribution variances
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double Time = tictac.Tac();
fprintf(stderr,"Time elapsed %.1lfms (%lf %lf %lf) \n",Time*1000,dm[0],dm[1],dm[2]);
isFirstLoad = false;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//If visualization desired
#ifdef USE_VISUALIZATION_DEBUG
///The sensor pose in the global frame for visualization
Eigen::Vector3d origin(dm[0] + L * cos(dm[2]+alpha),dm[1] + L * sin(dm[2]+alpha),0.1);
Eigen::Affine3d ppos = getAsAffine(dm[0],dm[1],dm[2]);
//lslgeneric::transformPointCloudInPlace(Tgt, *cloud);
lslgeneric::transformPointCloudInPlace(ppos, *cloud);
mrpt::opengl::COpenGLScenePtr &scene = win3D.get3DSceneAndLock();
win3D.setCameraPointingToPoint(gx,gy,1);
if(counter%2000==0) gl_points->clear();
scene->clear();
scene->insert(plane);
addMap2Scene(ndtmcl->map, origin, scene);
addPoseCovariance(dm[0],dm[1],cov,scene);
addScanToScene(scene, origin, cloud);
addParticlesToWorld(ndtmcl->pf,Tgt.translation(),dm, Todo.translation());
scene->insert(gl_points);
scene->insert(gl_particles);
win3D.unlockAccess3DScene();
win3D.repaint();
if (win3D.keyHit())
{
mrpt::gui::mrptKeyModifier kmods;
int key = win3D.getPushedKey(&kmods);
}
#endif
}
// Intersect triangle with bounding box. Return true if
// any of the faces of bb intersect triangle.
// Note: so returns false if triangle inside bb.
bool Foam::triangleFuncs::intersectBb
(
const point& p0,
const point& p1,
const point& p2,
const treeBoundBox& cubeBb
)
{
const vector p10 = p1 - p0;
const vector p20 = p2 - p0;
// cubeBb points; counted as if cell with vertex0 at cubeBb.min().
const point& min = cubeBb.min();
const point& max = cubeBb.max();
const point& cube0 = min;
const point cube1(min.x(), min.y(), max.z());
const point cube2(max.x(), min.y(), max.z());
const point cube3(max.x(), min.y(), min.z());
const point cube4(min.x(), max.y(), min.z());
const point cube5(min.x(), max.y(), max.z());
const point cube7(max.x(), max.y(), min.z());
//
// Intersect all 12 edges of cube with triangle
//
point pInter;
pointField origin(4);
// edges in x direction
origin[0] = cube0;
origin[1] = cube1;
origin[2] = cube5;
origin[3] = cube4;
scalar maxSx = max.x() - min.x();
if (intersectAxesBundle(p0, p10, p20, 0, origin, maxSx, pInter))
{
return true;
}
// edges in y direction
origin[0] = cube0;
origin[1] = cube1;
origin[2] = cube2;
origin[3] = cube3;
scalar maxSy = max.y() - min.y();
if (intersectAxesBundle(p0, p10, p20, 1, origin, maxSy, pInter))
{
return true;
}
// edges in z direction
origin[0] = cube0;
origin[1] = cube3;
origin[2] = cube7;
origin[3] = cube4;
scalar maxSz = max.z() - min.z();
if (intersectAxesBundle(p0, p10, p20, 2, origin, maxSz, pInter))
{
return true;
}
// Intersect triangle edges with bounding box
if (cubeBb.intersects(p0, p1, pInter))
{
return true;
}
if (cubeBb.intersects(p1, p2, pInter))
{
return true;
}
if (cubeBb.intersects(p2, p0, pInter))
{
return true;
}
return false;
}
/**
* @brief Pionek::boundingRect Zwraca ograniczenie pionka.
* @return najmniejszy prostokąt zawierający pionek
*/
QRectF PlayerMarker::boundingRect() const
{
QPointF origin(-radius_, -radius_);
QSizeF size(2 * radius_, 2 * radius_);
return QRectF(origin, size);
}
Mesh* MeshBuilder::GenerateTerrain(const std::string & meshName, const std::string &file_path, std::vector<unsigned char> &heightMap)
{
std::vector<Vertex> vertex_buffer_data;
std::vector<GLuint> index_buffer_data;
const float SCALE_FACTOR = 256.0f;
if (!LoadHeightMap(file_path.c_str(), heightMap))
{
return NULL;
}
Vertex v;
unsigned terrainSize = (unsigned)sqrt((double)heightMap.size());
for (unsigned z = 0; z < terrainSize; ++z)
{
for (unsigned x = 0; x < terrainSize; ++x)
{
float scaledHeight = (float)heightMap[z * terrainSize + x] / SCALE_FACTOR;
v.pos.Set(static_cast<float>(x) / terrainSize - 0.5f, scaledHeight, static_cast<float>(z) / terrainSize - 0.5f);
v.color.Set(scaledHeight, scaledHeight, scaledHeight); // for rendering height map without texture
v.texCoord.Set((float)x / terrainSize * 8, 1.f - (float)z / terrainSize * 8);
Vector3 origin((float)x, (float)(heightMap[z * terrainSize + x]), (float)z);
Vector3 U, V;
if (x + 1 < terrainSize && z + 1 < terrainSize)
{
U.Set((float)x + 1, (float)(heightMap[z * terrainSize + x + 1]), (float)z);
V.Set((float)x, (float)(heightMap[(z + 1) * terrainSize + x]), (float)z + 1);
v.normal = ((V - origin).Cross((U - origin))).Normalize();
}
else
{
v.normal.Set(0, 1, 0);
}
//std::cout << v.normal << std::endl;
vertex_buffer_data.push_back(v);
}
}
for (unsigned z = 0; z < terrainSize - 1; ++z)
{
for (unsigned x = 0; x < terrainSize - 1; ++x)
{
index_buffer_data.push_back(terrainSize * z + x + 0);
index_buffer_data.push_back(terrainSize *(z + 1) + x + 0);
index_buffer_data.push_back(terrainSize * z + x + 1);
index_buffer_data.push_back(terrainSize * (z + 1) + x + 1);
index_buffer_data.push_back(terrainSize * z + x + 1);
index_buffer_data.push_back(terrainSize * (z + 1) + x + 0);
}
}
Mesh *mesh = new Mesh(meshName);
glBindBuffer(GL_ARRAY_BUFFER, mesh->vertexBuffer);
glBufferData(GL_ARRAY_BUFFER, vertex_buffer_data.size() * sizeof(Vertex), &vertex_buffer_data[0], GL_STATIC_DRAW);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, mesh->indexBuffer);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, index_buffer_data.size() * sizeof(GLuint), &index_buffer_data[0], GL_STATIC_DRAW);
mesh->indexSize = index_buffer_data.size();
mesh->mode = Mesh::DRAW_TRIANGLES;
return mesh;
}
void HavokImport::createRagdollRigidBody(INode* n, INode* parent, INode* ragdollParent, bhkRigidBodyRef rbody) {
const int MaxChar = 512;
char buffer[MaxChar];
//TSTR name(A2THelper(buffer, parent->GetName().c_str(), _countof(buffer)));
n->SetName(FormatText(TEXT("Ragdoll_%s"), parent->GetName()));
Object *pObj = n->GetObjectRef();
IDerivedObject *dobj = nullptr;
if (n->SuperClassID() == GEN_DERIVOB_CLASS_ID)
dobj = static_cast<IDerivedObject*>(pObj);
else {
dobj = CreateDerivedObject(pObj);
}
MotionSystem msys = rbody->GetMotionSystem(); //?
MotionQuality qtype = rbody->GetQualityType();
float mass = rbody->GetMass();
float lindamp = rbody->GetLinearDamping();
float angdamp = rbody->GetAngularDamping();
float frict = rbody->GetFriction();
float resti = rbody->GetRestitution();
float maxlinvel = rbody->GetMaxLinearVelocity();
float maxangvel = rbody->GetMaxAngularVelocity();
float pendepth = rbody->GetPenetrationDepth();
InertiaMatrix im = rbody->GetInertia();
Modifier* rbMod = (Modifier*)CreateInstance(OSM_CLASS_ID, HK_RIGIDBODY_MODIFIER_CLASS_ID);
if (IParamBlock2* rbParameters = rbMod->GetParamBlockByID(PB_RB_MOD_PBLOCK)) {
//These are fundamental parameters
rbParameters->SetValue(PA_RB_MOD_MASS, 0, mass, 0);
rbParameters->SetValue(PA_RB_MOD_RESTITUTION, 0, resti, 0);
rbParameters->SetValue(PA_RB_MOD_FRICTION, 0, frict, 0);
rbParameters->SetValue(PA_RB_MOD_INERTIA_TENSOR, 0, Point3(im[0][0],im[1][1],im[2][2]), 0);
rbParameters->SetValue(PA_RB_MOD_LINEAR_DAMPING, 0, lindamp, 0);
rbParameters->SetValue(PA_RB_MOD_CHANGE_ANGULAR_DAMPING, 0, angdamp, 0);
rbParameters->SetValue(PA_RB_MOD_MAX_LINEAR_VELOCITY, 0, maxlinvel, 0);
rbParameters->SetValue(PA_RB_MOD_MAX_ANGULAR_VELOCITY, 0, maxangvel, 0);
rbParameters->SetValue(PA_RB_MOD_ALLOWED_PENETRATION_DEPTH, 0, pendepth, 0);
rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_FIXED, 0);
rbParameters->SetValue(PA_RB_MOD_SOLVER_DEACTIVATION, 0, SD_LOW, 0);
rbParameters->SetValue(PA_RB_MOD_DEACTIVATOR_TYPE, 0, DT_LOW, 0);
/*body->SetMotionSystem(MotionSystem::MO_SYS_BOX);
body->SetDeactivatorType(DeactivatorType::DEACTIVATOR_NEVER);
body->SetSolverDeactivation(SolverDeactivation::SOLVER_DEACTIVATION_LOW);
body->SetQualityType(MO_QUAL_FIXED);*/
/*switch (qtype) {
case MO_QUAL_INVALID:
break;
case MO_QUAL_FIXED:
rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_FIXED, 0);
break;
case MO_QUAL_KEYFRAMED:
rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_KEYFRAMED, 0);
break;
case MO_QUAL_DEBRIS:
rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_DEBRIS, 0);
break;
case MO_QUAL_MOVING:
rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_MOVING, 0);
break;
case MO_QUAL_CRITICAL:
rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_CRITICAL, 0);
break;
case MO_QUAL_BULLET:
rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_BULLET, 0);
break;
case MO_QUAL_USER:
break;
case MO_QUAL_CHARACTER:
break;
case MO_QUAL_KEYFRAMED_REPORT:
rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_KEYFRAMED_REPORTING, 0);
break;
}*/
}
//Link Rigid Body to parent Rigid Body
ICustAttribContainer* cc = rbMod->GetCustAttribContainer();
if (!cc)
{
rbMod->AllocCustAttribContainer();
cc = rbMod->GetCustAttribContainer();
}
CustAttrib* c = (CustAttrib*)CreateInstance(CUST_ATTRIB_CLASS_ID, Class_ID(0x6e663460, 0x32682c72));
IParamBlock2* custModParameters = c->GetParamBlock(0);
custModParameters->SetValue(0, 0, parent, 0);
cc->InsertCustAttrib(0, c);
Modifier* constraintMod = nullptr;
vector< bhkSerializableRef > constraints = rbody->GetConstraints();
//Rigid Body constraints
if (ragdollParent) {
for (vector< bhkSerializableRef >::iterator it = constraints.begin(); it != constraints.end(); ) {
bhkConstraintRef constraint = bhkConstraintRef(*it);
if (constraint->IsDerivedType(bhkLimitedHingeConstraint::TYPE)) {
bhkLimitedHingeConstraintRef limitedHingeConstraint = bhkLimitedHingeConstraintRef(*it);
LimitedHingeDescriptor lh = limitedHingeConstraint->GetLimitedHinge();
constraintMod = (Modifier*)CreateInstance(OSM_CLASS_ID, HK_CONSTRAINT_HINGE_CLASS_ID);
if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) {
constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_NODE, 0, ragdollParent, 0);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_ROTATION_LOCK, 0, 0, 0);
Point3 origin(0, 0, 0);
Matrix3 parentRotation(TOPOINT3(lh.perp2AxleInA1), TOPOINT3(lh.perp2AxleInA2), TOPOINT3(lh.axleA), origin);
Matrix3 childRotation(TOPOINT3(lh.perp2AxleInB1), TOPOINT3(lh.perp2AxleInB2), TOPOINT3(lh.axleB), origin);
//Matrix3 parentRotation(true);
//MatrixFromNormal(TOPOINT3(lh.axleA), parentRotation);
//Matrix3 childRotation(true);
//MatrixFromNormal(TOPOINT3(lh.axleB), childRotation);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, 0);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, 0);
}
if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_HINGE_MOD_PBLOCK)) {
constraintParameters->SetValue(PA_HINGE_MOD_IS_LIMITED, 0, 1, 0);
constraintParameters->SetValue(PA_HINGE_MOD_LIMIT_MIN, 0, TODEG(lh.minAngle), 0);
constraintParameters->SetValue(PA_HINGE_MOD_LIMIT_MAX, 0, TODEG(lh.maxAngle), 0);
constraintParameters->SetValue(PA_HINGE_MOD_MAX_FRICTION_TORQUE, 0, lh.maxFriction, 0);
// constraintParameters->SetValue(PA_HINGE_MOD_MOTOR_TYPE, 0, lh.motor., 0);
}
}
else if (constraint->IsDerivedType(bhkRagdollConstraint::TYPE)) {
bhkRagdollConstraintRef ragdollConstraint = bhkRagdollConstraintRef(*it);
RagdollDescriptor rag = ragdollConstraint->GetRagdoll();
constraintMod = (Modifier*)CreateInstance(OSM_CLASS_ID, HK_CONSTRAINT_RAGDOLL_CLASS_ID);
if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) {
constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_NODE, 0, ragdollParent, 0);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_ROTATION_LOCK, 0, 0, 0);
//TOVECTOR3(rag.twistA);
//MatrixFromNormal(TOPOINT3(rag.twistA), parentRotation);
Point3 origin(0,0,0);
Matrix3 parentRotation(TOPOINT3(rag.planeA),TOPOINT3(rag.motorA),TOPOINT3(rag.twistA),origin);
//TOVECTOR3(rag.twistB);
//MatrixFromNormal(TOPOINT3(rag.twistB), childRotation);
Matrix3 childRotation(TOPOINT3(rag.planeB), TOPOINT3(rag.motorB), TOPOINT3(rag.twistB), origin);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_TRANSLATION, 0, TOPOINT3(rag.pivotB), 0);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, 0);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_TRANSLATION, 0, TOPOINT3(rag.pivotA), 0);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, 0);
}
if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_RAGDOLL_MOD_PBLOCK)) {
constraintParameters->SetValue(PA_RAGDOLL_MOD_CONE_ANGLE, 0, TODEG(rag.coneMaxAngle), 0);
constraintParameters->SetValue(PA_RAGDOLL_MOD_PLANE_MIN, 0, TODEG(rag.planeMinAngle), 0);
constraintParameters->SetValue(PA_RAGDOLL_MOD_PLANE_MAX, 0, TODEG(rag.planeMaxAngle), 0);
constraintParameters->SetValue(PA_RAGDOLL_MOD_TWIST_MIN, 0, TODEG(rag.twistMinAngle), 0);
constraintParameters->SetValue(PA_RAGDOLL_MOD_TWIST_MAX, 0, TODEG(rag.twistMaxAngle), 0);
constraintParameters->SetValue(PA_RAGDOLL_MOD_MAX_FRICTION_TORQUE, 0, rag.maxFriction, 0);
}
}
else if (constraint->IsDerivedType(bhkMalleableConstraint::TYPE)) {
bhkMalleableConstraintRef malleableConstraint = bhkMalleableConstraintRef(*it);
if (malleableConstraint->GetConstraintType() == (unsigned int)2) {
LimitedHingeDescriptor lh = malleableConstraint->GetLimitedHinge();
constraintMod = (Modifier*)CreateInstance(OSM_CLASS_ID, HK_CONSTRAINT_HINGE_CLASS_ID);
if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) {
constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_NODE, 0, ragdollParent, 0);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_ROTATION_LOCK, 0, 0, 0);
Point3 origin(0, 0, 0);
Matrix3 parentRotation(TOPOINT3(lh.perp2AxleInA1), TOPOINT3(lh.perp2AxleInA2), TOPOINT3(lh.axleA), origin);
Matrix3 childRotation(TOPOINT3(lh.perp2AxleInB1), TOPOINT3(lh.perp2AxleInB2), TOPOINT3(lh.axleB), origin);
//Matrix3 parentRotation(true);
//MatrixFromNormal(TOPOINT3(lh.axleA), parentRotation);
//Matrix3 childRotation(true);
//MatrixFromNormal(TOPOINT3(lh.axleB), childRotation);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, 0);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, 0);
}
if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_HINGE_MOD_PBLOCK)) {
constraintParameters->SetValue(PA_HINGE_MOD_IS_LIMITED, 0, 1, 0);
constraintParameters->SetValue(PA_HINGE_MOD_LIMIT_MIN, 0, TODEG(lh.minAngle), 0);
constraintParameters->SetValue(PA_HINGE_MOD_LIMIT_MAX, 0, TODEG(lh.maxAngle), 0);
constraintParameters->SetValue(PA_HINGE_MOD_MAX_FRICTION_TORQUE, 0, lh.maxFriction, 0);
// constraintParameters->SetValue(PA_HINGE_MOD_MOTOR_TYPE, 0, lh.motor., 0);
}
}
else if (malleableConstraint->GetConstraintType() == (unsigned int)7) {
RagdollDescriptor rag = malleableConstraint->GetRagdoll();
constraintMod = (Modifier*)CreateInstance(OSM_CLASS_ID, HK_CONSTRAINT_RAGDOLL_CLASS_ID);
if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) {
constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_NODE, 0, ragdollParent, 0);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_ROTATION_LOCK, 0, 0, 0);
//TOVECTOR3(rag.twistA);
//MatrixFromNormal(TOPOINT3(rag.twistA), parentRotation);
Point3 origin(0, 0, 0);
Matrix3 parentRotation(TOPOINT3(rag.planeA), TOPOINT3(rag.motorA), TOPOINT3(rag.twistA), origin);
//TOVECTOR3(rag.twistB);
//MatrixFromNormal(TOPOINT3(rag.twistB), childRotation);
Matrix3 childRotation(TOPOINT3(rag.planeB), TOPOINT3(rag.motorB), TOPOINT3(rag.twistB), origin);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_TRANSLATION, 0, TOPOINT3(rag.pivotB), 0);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, 0);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_TRANSLATION, 0, TOPOINT3(rag.pivotA), 0);
constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, 0);
}
if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_RAGDOLL_MOD_PBLOCK)) {
constraintParameters->SetValue(PA_RAGDOLL_MOD_CONE_ANGLE, 0, TODEG(rag.coneMaxAngle), 0);
constraintParameters->SetValue(PA_RAGDOLL_MOD_PLANE_MIN, 0, TODEG(rag.planeMinAngle), 0);
constraintParameters->SetValue(PA_RAGDOLL_MOD_PLANE_MAX, 0, TODEG(rag.planeMaxAngle), 0);
constraintParameters->SetValue(PA_RAGDOLL_MOD_TWIST_MIN, 0, TODEG(rag.twistMinAngle), 0);
constraintParameters->SetValue(PA_RAGDOLL_MOD_TWIST_MAX, 0, TODEG(rag.twistMaxAngle), 0);
constraintParameters->SetValue(PA_RAGDOLL_MOD_MAX_FRICTION_TORQUE, 0, rag.maxFriction, 0);
}
}
}
++it;
}
}
dobj->SetAFlag(A_LOCK_TARGET);
dobj->AddModifier(rbMod);
if (constraintMod)
dobj->AddModifier(constraintMod);
dobj->ClearAFlag(A_LOCK_TARGET);
n->SetObjectRef(dobj);
}
Value* ConstDoubleValue::notEqualConstant(Procedure& proc, Value* other) const
{
if (!other->hasDouble())
return nullptr;
return proc.add<Const32Value>(origin(), m_value != other->asDouble());
}
Value* ConstDoubleValue::addConstant(Procedure& proc, int32_t other) const
{
return proc.add<ConstDoubleValue>(origin(), m_value + static_cast<double>(other));
}
position object::end()
{
return origin();
}
void Image::drawPattern(GraphicsContext* ctxt, const FloatRect& srcRect, const AffineTransform& patternTransform, const FloatPoint& phase, ColorSpace, CompositeOperator, const FloatRect& dstRect)
{
#if USE(WXGC)
wxGCDC* context = (wxGCDC*)ctxt->platformContext();
wxGraphicsBitmap* bitmap = nativeImageForCurrentFrame();
#else
wxWindowDC* context = ctxt->platformContext();
wxBitmap* bitmap = nativeImageForCurrentFrame();
#endif
if (!bitmap) // If it's too early we won't have an image yet.
return;
ctxt->save();
ctxt->clip(IntRect(dstRect.x(), dstRect.y(), dstRect.width(), dstRect.height()));
float currentW = 0;
float currentH = 0;
#if USE(WXGC)
wxGraphicsContext* gc = context->GetGraphicsContext();
float adjustedX = phase.x() + srcRect.x() *
narrowPrecisionToFloat(patternTransform.a());
float adjustedY = phase.y() + srcRect.y() *
narrowPrecisionToFloat(patternTransform.d());
gc->ConcatTransform(patternTransform);
#else
wxMemoryDC mydc;
mydc.SelectObject(*bitmap);
#endif
wxPoint origin(context->GetDeviceOrigin());
wxSize clientSize(context->GetSize());
while ( currentW < dstRect.width() && currentW < clientSize.x - origin.x ) {
while ( currentH < dstRect.height() && currentH < clientSize.y - origin.y) {
#if USE(WXGC)
#if wxCHECK_VERSION(2,9,0)
gc->DrawBitmap(*bitmap, adjustedX + currentW, adjustedY + currentH, (wxDouble)srcRect.width(), (wxDouble)srcRect.height());
#else
gc->DrawGraphicsBitmap(*bitmap, adjustedX + currentW, adjustedY + currentH, (wxDouble)srcRect.width(), (wxDouble)srcRect.height());
#endif
#else
context->Blit((wxCoord)dstRect.x() + currentW, (wxCoord)dstRect.y() + currentH,
(wxCoord)srcRect.width(), (wxCoord)srcRect.height(), &mydc,
(wxCoord)srcRect.x(), (wxCoord)srcRect.y(), wxCOPY, true);
#endif
currentH += srcRect.height();
}
currentW += srcRect.width();
currentH = 0;
}
ctxt->restore();
#if !USE(WXGC)
mydc.SelectObject(wxNullBitmap);
#endif
// NB: delete is causing crashes during page load, but not during the deletion
// itself. It occurs later on when a valid bitmap created in frameAtIndex
// suddenly becomes invalid after returning. It's possible these errors deal
// with reentrancy and threding problems.
//delete bitmap;
startAnimation();
if (ImageObserver* observer = imageObserver())
observer->didDraw(this);
}
position object::center()
{
return origin();
}
void ARMultiPublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg)
{
ARUint8 *dataPtr;
ARMarkerInfo *marker_info;
int marker_num;
int i, k, j;
/* Get the image from ROSTOPIC
* NOTE: the dataPtr format is BGR because the ARToolKit library was
* build with V4L, dataPtr format change according to the
* ARToolKit configure option (see config.h).*/
#if ROS_VERSION_MINIMUM(1, 9, 0)
try
{
capture_ = cv_bridge::toCvCopy (image_msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
}
dataPtr = (ARUint8 *) ((IplImage) capture_->image).imageData;
#else
try
{
capture_ = bridge_.imgMsgToCv (image_msg, "bgr8");
}
catch (sensor_msgs::CvBridgeException & e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
}
dataPtr = (ARUint8 *) capture_->imageData;
#endif
// detect the markers in the video frame
if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0)
{
argCleanup ();
ROS_BREAK ();
}
arPoseMarkers_.markers.clear ();
// check for known patterns
for (i = 0; i < objectnum; i++)
{
k = -1;
for (j = 0; j < marker_num; j++)
{
if (object[i].id == marker_info[j].id)
{
if (k == -1)
k = j;
else // make sure you have the best pattern (highest confidence factor)
if (marker_info[k].cf < marker_info[j].cf)
k = j;
}
}
if (k == -1)
{
object[i].visible = 0;
continue;
}
// calculate the transform for each marker
if (object[i].visible == 0)
{
arGetTransMat (&marker_info[k], object[i].marker_center, object[i].marker_width, object[i].trans);
}
else
{
arGetTransMatCont (&marker_info[k], object[i].trans,
object[i].marker_center, object[i].marker_width, object[i].trans);
}
object[i].visible = 1;
double arQuat[4], arPos[3];
//arUtilMatInv (object[i].trans, cam_trans);
arUtilMat2QuatPos (object[i].trans, arQuat, arPos);
// **** convert to ROS frame
double quat[4], pos[3];
pos[0] = arPos[0] * AR_TO_ROS;
pos[1] = arPos[1] * AR_TO_ROS;
pos[2] = arPos[2] * AR_TO_ROS;
if (isRightCamera_) {
pos[2] += 0; // -0.001704;
pos[0] += 0; // +0.0899971;
pos[1] += 0; // -0.00012;
}
quat[0] = -arQuat[0];
quat[1] = -arQuat[1];
quat[2] = -arQuat[2];
quat[3] = arQuat[3];
ROS_DEBUG (" Object num %i ------------------------------------------------", i);
ROS_DEBUG (" QUAT: Pos x: %3.5f y: %3.5f z: %3.5f", pos[0], pos[1], pos[2]);
ROS_DEBUG (" Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);
// **** publish the marker
ar_pose::ARMarker ar_pose_marker;
ar_pose_marker.header.frame_id = image_msg->header.frame_id;
ar_pose_marker.header.stamp = image_msg->header.stamp;
ar_pose_marker.id = object[i].id;
ar_pose_marker.pose.pose.position.x = pos[0];
ar_pose_marker.pose.pose.position.y = pos[1];
ar_pose_marker.pose.pose.position.z = pos[2];
ar_pose_marker.pose.pose.orientation.x = quat[0];
ar_pose_marker.pose.pose.orientation.y = quat[1];
ar_pose_marker.pose.pose.orientation.z = quat[2];
ar_pose_marker.pose.pose.orientation.w = quat[3];
ar_pose_marker.confidence = round(marker_info[k].cf * 100);
arPoseMarkers_.markers.push_back (ar_pose_marker);
// **** publish transform between camera and marker
#if ROS_VERSION_MINIMUM(1, 9, 0)
btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
btVector3 origin (pos[0], pos[1], pos[2]);
btTransform t (rotation, origin);
#else
// DEPRECATED: Fuerte support ends when Hydro is released
btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
btVector3 origin (pos[0], pos[1], pos[2]);
btTransform t (rotation, origin);
#endif
if (publishTf_)
{
std::string name = object[i].name;
if (isRightCamera_) {
name += "_r";
}
tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, name);
broadcaster_.sendTransform(camToMarker);
}
// **** publish visual marker
if (publishVisualMarkers_)
{
#if ROS_VERSION_MINIMUM(1, 9, 0)
btVector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
btTransform m (btQuaternion::getIdentity (), markerOrigin);
btTransform markerPose = t * m; // marker pose in the camera frame
#else
// DEPRECATED: Fuerte support ends when Hydro is released
btVector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
btTransform m (btQuaternion::getIdentity (), markerOrigin);
btTransform markerPose = t * m; // marker pose in the camera frame
#endif
tf::poseTFToMsg (markerPose, rvizMarker_.pose);
rvizMarker_.header.frame_id = image_msg->header.frame_id;
rvizMarker_.header.stamp = image_msg->header.stamp;
rvizMarker_.id = object[i].id;
rvizMarker_.scale.x = 1.0 * object[i].marker_width * AR_TO_ROS;
rvizMarker_.scale.y = 1.0 * object[i].marker_width * AR_TO_ROS;
rvizMarker_.scale.z = 0.5 * object[i].marker_width * AR_TO_ROS;
rvizMarker_.ns = "basic_shapes";
rvizMarker_.type = visualization_msgs::Marker::CUBE;
rvizMarker_.action = visualization_msgs::Marker::ADD;
switch (i)
{
case 0:
rvizMarker_.color.r = 0.0f;
rvizMarker_.color.g = 0.0f;
rvizMarker_.color.b = 1.0f;
rvizMarker_.color.a = 1.0;
break;
case 1:
rvizMarker_.color.r = 1.0f;
rvizMarker_.color.g = 0.0f;
rvizMarker_.color.b = 0.0f;
rvizMarker_.color.a = 1.0;
break;
default:
rvizMarker_.color.r = 0.0f;
rvizMarker_.color.g = 1.0f;
rvizMarker_.color.b = 0.0f;
rvizMarker_.color.a = 1.0;
}
rvizMarker_.lifetime = ros::Duration (1.0);
rvizMarkerPub_.publish(rvizMarker_);
ROS_DEBUG ("Published visual marker");
}
}
arMarkerPub_.publish(arPoseMarkers_);
ROS_DEBUG ("Published ar_multi markers");
}
void ARSinglePublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg)
{
ARUint8 *dataPtr;
ARMarkerInfo *marker_info;
int marker_num;
int i, k;
/* Get the image from ROSTOPIC
* NOTE: the dataPtr format is BGR because the ARToolKit library was
* build with V4L, dataPtr format change according to the
* ARToolKit configure option (see config.h).*/
try
{
capture_ = bridge_.imgMsgToCv (image_msg, "bgr8");
}
catch (sensor_msgs::CvBridgeException & e)
{
ROS_ERROR ("Could not convert from '%s' to 'bgr8'.", image_msg->encoding.c_str ());
}
//cvConvertImage(capture_,capture_,CV_CVTIMG_FLIP); //flip image
dataPtr = (ARUint8 *) capture_->imageData;
// detect the markers in the video frame
if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0)
{
ROS_FATAL ("arDetectMarker failed");
ROS_BREAK (); // FIXME: I don't think this should be fatal... -Bill
}
// check for known patterns
k = -1;
for (i = 0; i < marker_num; i++)
{
if (marker_info[i].id == patt_id_)
{
ROS_DEBUG ("Found pattern: %d ", patt_id_);
// make sure you have the best pattern (highest confidence factor)
if (k == -1)
k = i;
else if (marker_info[k].cf < marker_info[i].cf)
k = i;
}
}
if (k != -1)
{
// **** get the transformation between the marker and the real camera
double arQuat[4], arPos[3];
if (!useHistory_ || contF == 0)
arGetTransMat (&marker_info[k], marker_center_, markerWidth_, marker_trans_);
else
arGetTransMatCont (&marker_info[k], marker_trans_, marker_center_, markerWidth_, marker_trans_);
contF = 1;
//arUtilMatInv (marker_trans_, cam_trans);
arUtilMat2QuatPos (marker_trans_, arQuat, arPos);
// **** convert to ROS frame
double quat[4], pos[3];
pos[0] = arPos[0] * AR_TO_ROS;
pos[1] = arPos[1] * AR_TO_ROS;
pos[2] = arPos[2] * AR_TO_ROS;
quat[0] = -arQuat[0];
quat[1] = -arQuat[1];
quat[2] = -arQuat[2];
quat[3] = arQuat[3];
ROS_DEBUG (" QUAT: Pos x: %3.5f y: %3.5f z: %3.5f", pos[0], pos[1], pos[2]);
ROS_DEBUG (" Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);
// **** publish the marker
ar_pose_marker_.header.frame_id = image_msg->header.frame_id;
ar_pose_marker_.header.stamp = image_msg->header.stamp;
ar_pose_marker_.id = marker_info->id;
ar_pose_marker_.pose.pose.position.x = pos[0];
ar_pose_marker_.pose.pose.position.y = pos[1];
ar_pose_marker_.pose.pose.position.z = pos[2];
ar_pose_marker_.pose.pose.orientation.x = quat[0];
ar_pose_marker_.pose.pose.orientation.y = quat[1];
ar_pose_marker_.pose.pose.orientation.z = quat[2];
ar_pose_marker_.pose.pose.orientation.w = quat[3];
ar_pose_marker_.confidence = marker_info->cf;
arMarkerPub_.publish(ar_pose_marker_);
ROS_DEBUG ("Published ar_single marker");
// **** publish transform between camera and marker
btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
btVector3 origin(pos[0], pos[1], pos[2]);
btTransform t(rotation, origin);
if(publishTf_)
{
if(reverse_transform_)
{
tf::StampedTransform markerToCam (t.inverse(), image_msg->header.stamp, markerFrame_.c_str(), image_msg->header.frame_id);
broadcaster_.sendTransform(markerToCam);
} else {
tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame_.c_str());
broadcaster_.sendTransform(camToMarker);
}
}
// **** publish visual marker
if(publishVisualMarkers_)
{
btVector3 markerOrigin(0, 0, 0.25 * markerWidth_ * AR_TO_ROS);
btTransform m(btQuaternion::getIdentity(), markerOrigin);
btTransform markerPose = t * m; // marker pose in the camera frame
tf::poseTFToMsg(markerPose, rvizMarker_.pose);
rvizMarker_.header.frame_id = image_msg->header.frame_id;
rvizMarker_.header.stamp = image_msg->header.stamp;
rvizMarker_.id = 1;
rvizMarker_.scale.x = 1.0 * markerWidth_ * AR_TO_ROS;
rvizMarker_.scale.y = 1.0 * markerWidth_ * AR_TO_ROS;
rvizMarker_.scale.z = 0.5 * markerWidth_ * AR_TO_ROS;
rvizMarker_.ns = "basic_shapes";
rvizMarker_.type = visualization_msgs::Marker::CUBE;
rvizMarker_.action = visualization_msgs::Marker::ADD;
rvizMarker_.color.r = 0.0f;
rvizMarker_.color.g = 1.0f;
rvizMarker_.color.b = 0.0f;
rvizMarker_.color.a = 1.0;
rvizMarker_.lifetime = ros::Duration();
rvizMarkerPub_.publish(rvizMarker_);
ROS_DEBUG ("Published visual marker");
}
}
else
{
contF = 0;
ROS_DEBUG ("Failed to locate marker");
}
}
position object::north()
{
return origin();
}
static
bool test_route(const unsigned n_airspaces, const RasterMap& map)
{
airspaces = new Airspaces;
setup_airspaces(*airspaces, map.GetMapCenter(), n_airspaces);
{
std::ofstream fout ("results/terrain.txt");
unsigned nx = 100;
unsigned ny = 100;
GeoPoint origin(map.GetMapCenter());
for (unsigned i=0; i< nx; ++i) {
for (unsigned j=0; j< ny; ++j) {
fixed fx = (fixed)i/(nx-1)*fixed(2.0)-fixed_one;
fixed fy = (fixed)j/(ny-1)*fixed(2.0)-fixed_one;
GeoPoint x(origin.Longitude+Angle::degrees(fixed(0.2)+fixed(0.7)*fx),
origin.Latitude+Angle::degrees(fixed(0.9)*fy));
short h = map.GetInterpolatedHeight(x);
fout << x.Longitude.value_degrees() << " " << x.Latitude.value_degrees() << " " << h << "\n";
}
fout << "\n";
}
fout << "\n";
}
{ // local scope, see what happens when we go out of scope
GeoPoint p_start(Angle::degrees(fixed(-0.3)),Angle::degrees(fixed(0.0))); p_start += map.GetMapCenter();
GeoPoint p_dest(Angle::degrees(fixed(0.8)),Angle::degrees(fixed(-0.7))); p_dest += map.GetMapCenter();
AGeoPoint loc_start(p_start,map.GetHeight(p_start)+100);
AGeoPoint loc_end(p_dest,map.GetHeight(p_dest)+100);
AIRCRAFT_STATE state;
GlidePolar glide_polar(fixed(0.1));
AirspaceAircraftPerformanceGlide perf(glide_polar);
GeoVector vec(loc_start, loc_end);
fixed range = fixed(10000)+ vec.Distance / 2;
state.Location = loc_start;
state.NavAltitude = fixed(loc_start.altitude);
state.AirspaceAltitude = fixed(loc_start.altitude);
{
Airspaces as_route(*airspaces, false);
// dummy
// real one, see if items changed
as_route.synchronise_in_range(*airspaces, vec.mid_point(loc_start), range);
if (verbose)
printf("# route airspace size %d\n", as_route.size());
int size_1 = as_route.size();
as_route.synchronise_in_range(*airspaces, vec.mid_point(loc_start), fixed_one);
if (verbose)
printf("# route airspace size %d\n", as_route.size());
int size_2 = as_route.size();
ok(size_2<size_1,"shrink as",0);
// go back
as_route.synchronise_in_range(*airspaces, vec.mid_point(loc_end), range);
if (verbose)
printf("# route airspace size %d\n", as_route.size());
int size_3 = as_route.size();
ok(size_3>=size_2,"grow as",0);
// and again
as_route.synchronise_in_range(*airspaces, vec.mid_point(loc_start), range);
if (verbose)
printf("# route airspace size %d\n", as_route.size());
int size_4 = as_route.size();
ok(size_4>=size_3,"grow as",0);
scan_airspaces(state, as_route, perf, true, loc_end);
}
// try the solver
SpeedVector wind(Angle::degrees(fixed(0)), fixed(0.0));
GlidePolar polar(fixed_one);
AirspaceRoute route(polar, wind, *airspaces);
route.set_terrain(&map);
RoutePlannerConfig config;
config.mode = RoutePlannerConfig::rpBoth;
bool sol = false;
for (int i=0; i<NUM_SOL; i++) {
loc_end.Latitude+= Angle::degrees(fixed(0.1));
loc_end.altitude = map.GetHeight(loc_end)+100;
route.synchronise(*airspaces, loc_start, loc_end);
if (route.solve(loc_start, loc_end, config)) {
sol = true;
if (verbose) {
PrintHelper::print_route(route);
}
} else {
if (verbose) {
printf("# fail\n");
}
sol = false;
}
char buffer[80];
sprintf(buffer,"route %d solution", i);
ok(sol, buffer, 0);
}
}
delete airspaces; airspaces = NULL;
return true;
}
position object::west()
{
return origin();
}
//------------------------------------------------------------------------------
LimitStencilTable const *
LimitStencilTableFactory::Create(TopologyRefiner const & refiner,
LocationArrayVec const & locationArrays,
StencilTable const * cvStencilsIn,
PatchTable const * patchTableIn,
Options options) {
// Compute the total number of stencils to generate
int numStencils=0, numLimitStencils=0;
for (int i=0; i<(int)locationArrays.size(); ++i) {
assert(locationArrays[i].numLocations>=0);
numStencils += locationArrays[i].numLocations;
}
if (numStencils<=0) {
return 0;
}
bool uniform = refiner.IsUniform();
int maxlevel = refiner.GetMaxLevel();
StencilTable const * cvstencils = cvStencilsIn;
if (! cvstencils) {
// Generate stencils for the control vertices - this is necessary to
// properly factorize patches with control vertices at level 0 (natural
// regular patches, such as in a torus)
// note: the control vertices of the mesh are added as single-index
// stencils of weight 1.0f
StencilTableFactory::Options options;
options.generateIntermediateLevels = uniform ? false :true;
options.generateControlVerts = true;
options.generateOffsets = true;
// PERFORMANCE: We could potentially save some mem-copies by not
// instantiating the stencil tables and work directly off the source
// data.
cvstencils = StencilTableFactory::Create(refiner, options);
} else {
// Sanity checks
//
// Note that the input cvStencils could be larger than the number of
// refiner's vertices, due to the existence of the end cap stencils.
if (cvstencils->GetNumStencils() < (uniform ?
refiner.GetLevel(maxlevel).GetNumVertices() :
refiner.GetNumVerticesTotal())) {
return 0;
}
}
// If a stencil table was given, use it, otherwise, create a new one
PatchTable const * patchtable = patchTableIn;
if (! patchtable) {
// XXXX (manuelk) If no patch-table was passed, we should be able to
// infer the patches fairly easily from the refiner. Once more tags
// have been added to the refiner, maybe we can remove the need for the
// patch table.
PatchTableFactory::Options options;
options.SetEndCapType(
Far::PatchTableFactory::Options::ENDCAP_GREGORY_BASIS);
patchtable = PatchTableFactory::Create(refiner, options);
if (! cvStencilsIn) {
// if cvstencils is just created above, append endcap stencils
if (StencilTable const *localPointStencilTable =
patchtable->GetLocalPointStencilTable()) {
StencilTable const *table =
StencilTableFactory::AppendLocalPointStencilTable(
refiner, cvstencils, localPointStencilTable);
delete cvstencils;
cvstencils = table;
}
}
} else {
// Sanity checks
if (patchtable->IsFeatureAdaptive()==uniform) {
if (! cvStencilsIn) {
assert(cvstencils && cvstencils!=cvStencilsIn);
delete cvstencils;
}
return 0;
}
}
assert(patchtable && cvstencils);
// Create a patch-map to locate sub-patches faster
PatchMap patchmap( *patchtable );
//
// Generate limit stencils for locations
//
internal::StencilBuilder builder(refiner.GetLevel(0).GetNumVertices(),
/*genControlVerts*/ false,
/*compactWeights*/ true);
internal::StencilBuilder::Index origin(&builder, 0);
internal::StencilBuilder::Index dst = origin;
float wP[20], wDs[20], wDt[20], wDss[20], wDst[20], wDtt[20];
for (size_t i=0; i<locationArrays.size(); ++i) {
LocationArray const & array = locationArrays[i];
assert(array.ptexIdx>=0);
for (int j=0; j<array.numLocations; ++j) { // for each face we're working on
float s = array.s[j],
t = array.t[j]; // for each target (s,t) point on that face
PatchMap::Handle const * handle =
patchmap.FindPatch(array.ptexIdx, s, t);
if (handle) {
ConstIndexArray cvs = patchtable->GetPatchVertices(*handle);
StencilTable const & src = *cvstencils;
dst = origin[numLimitStencils];
if (options.generate2ndDerivatives) {
patchtable->EvaluateBasis(*handle, s, t, wP, wDs, wDt, wDss, wDst, wDtt);
dst.Clear();
for (int k = 0; k < cvs.size(); ++k) {
dst.AddWithWeight(src[cvs[k]], wP[k], wDs[k], wDt[k], wDss[k], wDst[k], wDtt[k]);
}
} else if (options.generate1stDerivatives) {
patchtable->EvaluateBasis(*handle, s, t, wP, wDs, wDt);
dst.Clear();
for (int k = 0; k < cvs.size(); ++k) {
dst.AddWithWeight(src[cvs[k]], wP[k], wDs[k], wDt[k]);
}
} else {
patchtable->EvaluateBasis(*handle, s, t, wP);
dst.Clear();
for (int k = 0; k < cvs.size(); ++k) {
dst.AddWithWeight(src[cvs[k]], wP[k]);
}
}
++numLimitStencils;
}
}
}
if (! cvStencilsIn) {
delete cvstencils;
}
if (! patchTableIn) {
delete patchtable;
}
//
// Copy the proto-stencils into the limit stencil table
//
LimitStencilTable * result = new LimitStencilTable(
refiner.GetLevel(0).GetNumVertices(),
builder.GetStencilOffsets(),
builder.GetStencilSizes(),
builder.GetStencilSources(),
builder.GetStencilWeights(),
builder.GetStencilDuWeights(),
builder.GetStencilDvWeights(),
builder.GetStencilDuuWeights(),
builder.GetStencilDuvWeights(),
builder.GetStencilDvvWeights(),
/*ctrlVerts*/false,
/*fristOffset*/0);
return result;
}
position object::north_east()
{
return origin();
}
/*!
\property QGeoMapPixmapObject::coordinate
\brief This property holds the coordinate that specifies where the pixmap
will be drawn by this pixmap object.
The default value of this property is an invalid coordinate. While the
value of this property is invalid the pixmap object will not be displayed.
*/
QGeoCoordinate QGeoMapPixmapObject::coordinate() const
{
return origin();
}
position object::south_west()
{
return origin();
}
Object *
BuildNURBSTorus(float radius, float radius2, BOOL sliceon, float pie1, float pie2, BOOL genUVs)
{
NURBSSet nset;
Point3 origin(0,0,0);
Point3 symAxis(0,0,1);
Point3 refAxis(0,1,0);
float startAngle = 0.0f;
float endAngle = TWOPI;
if (sliceon && pie1 != pie2) {
float sweep = pie2-pie1;
if (sweep <= 0.0f) sweep += TWOPI;
refAxis = Point3(Point3(0,1,0) * RotateZMatrix(pie1));
endAngle = sweep;
}
// first the main surface
NURBSCVSurface *surf = new NURBSCVSurface();
nset.AppendObject(surf);
surf->SetGenerateUVs(genUVs);
surf->SetTextureUVs(0, 0, Point2(0.0f, 0.0f));
surf->SetTextureUVs(0, 1, Point2(0.0f, 1.0f));
surf->SetTextureUVs(0, 2, Point2(1.0f, 0.0f));
surf->SetTextureUVs(0, 3, Point2(1.0f, 1.0f));
surf->FlipNormals(TRUE);
surf->Renderable(TRUE);
TCHAR bname[80];
TCHAR sname[80];
_tcscpy(bname, GetString(IDS_RB_TORUS));
_stprintf(sname, _T("%s%s"), bname, GetString(IDS_CT_SURF));
surf->SetName(sname);
if (sliceon && pie1 != pie2) {
GenNURBSTorusSurface(radius, radius2, origin, symAxis, refAxis,
startAngle, endAngle, -PI, PI, TRUE, *surf);
// now create caps on the ends
NURBSCapSurface *cap0 = new NURBSCapSurface();
nset.AppendObject(cap0);
cap0->SetGenerateUVs(genUVs);
cap0->SetParent(0);
cap0->SetEdge(2);
cap0->FlipNormals(FALSE);
cap0->Renderable(TRUE);
TCHAR sname[80];
_stprintf(sname, _T("%s%s%02d"), bname, GetString(IDS_CT_CAP), 0);
cap0->SetName(sname);
NURBSCapSurface *cap1 = new NURBSCapSurface();
nset.AppendObject(cap1);
cap1->SetGenerateUVs(genUVs);
cap1->SetParent(0);
cap1->SetEdge(3);
cap1->FlipNormals(TRUE);
cap1->Renderable(TRUE);
_stprintf(sname, _T("%s%s%02d"), bname, GetString(IDS_CT_CAP), 1);
cap1->SetName(sname);
} else {
GenNURBSTorusSurface(radius, radius2, origin, symAxis, refAxis,
startAngle, endAngle, -PI, PI, FALSE, *surf);
}
Matrix3 mat;
mat.IdentityMatrix();
Object *ob = CreateNURBSObject(NULL, &nset, mat);
return ob;
}
position object::start()
{
return origin();
}
void soundManagerApp::myDraw()
{
//std::cout << "\n--- myDraw() ---\n";
//std::cout << "HeadPos:" << vrj::Coord(*mHead->getData()).pos << "\t"
// << "WandPos:" << vrj::Coord(*mWand->getData()).pos << std::endl;
glMatrixMode(GL_MODELVIEW);
// -- Draw box on wand --- //
gmtl::Matrix44f wandMatrix = mWand->getData(); // Get the wand matrix
glPushMatrix();
// cout << "wand:\n" << *wandMatrix << endl;
glPushMatrix();
glMultMatrixf(wandMatrix.mData); // Push the wand matrix on the stack
//glColor3f(1.0f, 0.0f, 1.0f);
float wand_color[3];
wand_color[0] = wand_color[1] = wand_color[2] = 0.0f;
if(mButton0->getData() == gadget::Digital::ON)
wand_color[0] += 0.5f;
if(mButton1->getData() == gadget::Digital::ON)
wand_color[1] += 0.5f;
if(mButton2->getData() == gadget::Digital::ON)
wand_color[2] += 0.5f;
if(mButton3->getData() == gadget::Digital::ON)
wand_color[0] += 0.5f;
if(mButton4->getData() == gadget::Digital::ON)
wand_color[1] += 0.5f;
if(mButton5->getData() == gadget::Digital::ON)
wand_color[2] += 0.5f;
glColor3fv(wand_color);
glScalef(0.25f, 0.25f, 0.25f);
drawCube();
glPopMatrix();
// A little laser pointer
glLineWidth(5.0f);
// Draw Axis
glDisable(GL_LIGHTING);
glPushMatrix();
glMultMatrixf(wandMatrix.mData); // Goto wand position
gmtl::Vec3f x_axis(7.0f,0.0f,0.0f);
gmtl::Vec3f y_axis(0.0f, 7.0f, 0.0f);
gmtl::Vec3f z_axis(0.0f, 0.0f, 7.0f);
gmtl::Vec3f origin(0.0f, 0.0f, 0.0f);
glBegin(GL_LINES);
glColor3f(1.0f, 0.0f, 0.0f);
glVertex3fv(origin.mData);
glVertex3fv(x_axis.mData);
glColor3f(0.0f, 1.0f, 0.0f);
glVertex3fv(origin.mData);
glVertex3fv(y_axis.mData);
glColor3f(0.0f, 0.0f, 1.0f);
glVertex3fv(origin.mData);
glVertex3fv(z_axis.mData);
glEnd();
glPopMatrix();
glEnable(GL_LIGHTING);
glPopMatrix();
}
S32 LLFontGL::render(const LLWString &wstr, S32 begin_offset, F32 x, F32 y, const LLColor4 &color, HAlign halign, VAlign valign, U8 style,
ShadowType shadow, S32 max_chars, S32 max_pixels, F32* right_x, BOOL use_ellipses) const
{
LLFastTimer _(FTM_RENDER_FONTS);
if(!sDisplayFont) //do not display texts
{
return wstr.length() ;
}
if (wstr.empty())
{
return 0;
}
gGL.getTexUnit(0)->enable(LLTexUnit::TT_TEXTURE);
S32 scaled_max_pixels = max_pixels == S32_MAX ? S32_MAX : llceil((F32)max_pixels * sScaleX);
// determine which style flags need to be added programmatically by stripping off the
// style bits that are drawn by the underlying Freetype font
U8 style_to_add = (style | mFontDescriptor.getStyle()) & ~mFontFreetype->getStyle();
F32 drop_shadow_strength = 0.f;
if (shadow != NO_SHADOW)
{
F32 luminance;
color.calcHSL(NULL, NULL, &luminance);
drop_shadow_strength = clamp_rescale(luminance, 0.35f, 0.6f, 0.f, 1.f);
if (luminance < 0.35f)
{
shadow = NO_SHADOW;
}
}
gGL.pushUIMatrix();
gGL.loadUIIdentity();
LLVector2 origin(floorf(sCurOrigin.mX*sScaleX), floorf(sCurOrigin.mY*sScaleY));
// snap the text origin to a pixel grid to start with
origin.mV[VX] -= ll_round((F32)sCurOrigin.mX) - (sCurOrigin.mX);
origin.mV[VY] -= ll_round((F32)sCurOrigin.mY) - (sCurOrigin.mY);
// Depth translation, so that floating text appears 'inworld'
// and is correclty occluded.
gGL.translatef(0.f,0.f,sCurOrigin.mZ);
S32 chars_drawn = 0;
S32 i;
S32 length;
if (-1 == max_chars)
{
length = (S32)wstr.length() - begin_offset;
}
else
{
length = llmin((S32)wstr.length() - begin_offset, max_chars );
}
F32 cur_x, cur_y, cur_render_x, cur_render_y;
// Not guaranteed to be set correctly
gGL.setSceneBlendType(LLRender::BT_ALPHA);
cur_x = ((F32)x * sScaleX) + origin.mV[VX];
cur_y = ((F32)y * sScaleY) + origin.mV[VY];
// Offset y by vertical alignment.
// use unscaled font metrics here
switch (valign)
{
case TOP:
cur_y -= mFontFreetype->getAscenderHeight();
break;
case BOTTOM:
cur_y += mFontFreetype->getDescenderHeight();
break;
case VCENTER:
cur_y -= (mFontFreetype->getAscenderHeight() - mFontFreetype->getDescenderHeight()) / 2.f;
break;
case BASELINE:
// Baseline, do nothing.
break;
default:
break;
}
switch (halign)
{
case LEFT:
break;
case RIGHT:
cur_x -= llmin(scaled_max_pixels, ll_round(getWidthF32(wstr.c_str(), begin_offset, length) * sScaleX));
break;
case HCENTER:
cur_x -= llmin(scaled_max_pixels, ll_round(getWidthF32(wstr.c_str(), begin_offset, length) * sScaleX)) / 2;
break;
default:
break;
}
cur_render_y = cur_y;
cur_render_x = cur_x;
F32 start_x = (F32)ll_round(cur_x);
const LLFontBitmapCache* font_bitmap_cache = mFontFreetype->getFontBitmapCache();
F32 inv_width = 1.f / font_bitmap_cache->getBitmapWidth();
F32 inv_height = 1.f / font_bitmap_cache->getBitmapHeight();
const S32 LAST_CHARACTER = LLFontFreetype::LAST_CHAR_FULL;
BOOL draw_ellipses = FALSE;
if (use_ellipses)
{
// check for too long of a string
S32 string_width = ll_round(getWidthF32(wstr.c_str(), begin_offset, max_chars) * sScaleX);
if (string_width > scaled_max_pixels)
{
// use four dots for ellipsis width to generate padding
const LLWString dots(utf8str_to_wstring(std::string("....")));
scaled_max_pixels = llmax(0, scaled_max_pixels - ll_round(getWidthF32(dots.c_str())));
draw_ellipses = TRUE;
}
}
const LLFontGlyphInfo* next_glyph = NULL;
const S32 GLYPH_BATCH_SIZE = 30;
LLVector4a vertices[GLYPH_BATCH_SIZE * 4];
LLVector2 uvs[GLYPH_BATCH_SIZE * 4];
LLColor4U colors[GLYPH_BATCH_SIZE * 4];
LLColor4U text_color(color);
S32 bitmap_num = -1;
S32 glyph_count = 0;
for (i = begin_offset; i < begin_offset + length; i++)
{
llwchar wch = wstr[i];
const LLFontGlyphInfo* fgi = next_glyph;
next_glyph = NULL;
if(!fgi)
{
fgi = mFontFreetype->getGlyphInfo(wch);
}
if (!fgi)
{
LL_ERRS() << "Missing Glyph Info" << LL_ENDL;
break;
}
// Per-glyph bitmap texture.
S32 next_bitmap_num = fgi->mBitmapNum;
if (next_bitmap_num != bitmap_num)
{
// Actually draw the queued glyphs before switching their texture;
// otherwise the queued glyphs will be taken from wrong textures.
if (glyph_count > 0)
{
gGL.begin(LLRender::QUADS);
{
gGL.vertexBatchPreTransformed(vertices, uvs, colors, glyph_count * 4);
}
gGL.end();
glyph_count = 0;
}
bitmap_num = next_bitmap_num;
LLImageGL *font_image = font_bitmap_cache->getImageGL(bitmap_num);
gGL.getTexUnit(0)->bind(font_image);
}
if ((start_x + scaled_max_pixels) < (cur_x + fgi->mXBearing + fgi->mWidth))
{
// Not enough room for this character.
break;
}
// Draw the text at the appropriate location
//Specify vertices and texture coordinates
LLRectf uv_rect((fgi->mXBitmapOffset) * inv_width,
(fgi->mYBitmapOffset + fgi->mHeight + PAD_UVY) * inv_height,
(fgi->mXBitmapOffset + fgi->mWidth) * inv_width,
(fgi->mYBitmapOffset - PAD_UVY) * inv_height);
// snap glyph origin to whole screen pixel
LLRectf screen_rect((F32)ll_round(cur_render_x + (F32)fgi->mXBearing),
(F32)ll_round(cur_render_y + (F32)fgi->mYBearing),
(F32)ll_round(cur_render_x + (F32)fgi->mXBearing) + (F32)fgi->mWidth,
(F32)ll_round(cur_render_y + (F32)fgi->mYBearing) - (F32)fgi->mHeight);
if (glyph_count >= GLYPH_BATCH_SIZE)
{
gGL.begin(LLRender::QUADS);
{
gGL.vertexBatchPreTransformed(vertices, uvs, colors, glyph_count * 4);
}
gGL.end();
glyph_count = 0;
}
drawGlyph(glyph_count, vertices, uvs, colors, screen_rect, uv_rect, text_color, style_to_add, shadow, drop_shadow_strength);
chars_drawn++;
cur_x += fgi->mXAdvance;
cur_y += fgi->mYAdvance;
llwchar next_char = wstr[i+1];
if (next_char && (next_char < LAST_CHARACTER))
{
// Kern this puppy.
next_glyph = mFontFreetype->getGlyphInfo(next_char);
cur_x += mFontFreetype->getXKerning(fgi, next_glyph);
}
// Round after kerning.
// Must do this to cur_x, not just to cur_render_x, otherwise you
// will squish sub-pixel kerned characters too close together.
// For example, "CCCCC" looks bad.
cur_x = (F32)ll_round(cur_x);
//cur_y = (F32)ll_round(cur_y);
cur_render_x = cur_x;
cur_render_y = cur_y;
}
if (glyph_count > 0)
{
gGL.begin(LLRender::QUADS);
{
gGL.vertexBatchPreTransformed(vertices, uvs, colors, glyph_count * 4);
}
gGL.end();
}
if (right_x)
{
*right_x = (cur_x - origin.mV[VX]) / sScaleX;
}
//FIXME: add underline as glyph?
if (style_to_add & UNDERLINE)
{
F32 descender = mFontFreetype->getDescenderHeight();
gGL.getTexUnit(0)->unbind(LLTexUnit::TT_TEXTURE);
gGL.begin(LLRender::LINES);
gGL.vertex2f(start_x, cur_y - descender);
gGL.vertex2f(cur_x, cur_y - descender);
gGL.end();
}
if (draw_ellipses)
{
// recursively render ellipses at end of string
// we've already reserved enough room
gGL.pushUIMatrix();
renderUTF8(std::string("..."),
0,
(cur_x - origin.mV[VX]) / sScaleX, (F32)y,
color,
LEFT, valign,
style_to_add,
shadow,
S32_MAX, max_pixels,
right_x,
FALSE);
gGL.popUIMatrix();
}
gGL.popUIMatrix();
return chars_drawn;
}
