RDirNode::RDirNode(RDirNode* parent, std::string abspath) {
changePath(abspath);
parent = 0;
setParent(parent);
//figure out starting position
if(parent !=0) {
vec2f parentPos = parent->getPos();
vec2f offset;
RDirNode* parentP = parent->getParent();
pos = parentPos;
} else {
pos = vec2f(0.0f, 0.0f);
}
float padded_file_radius = gGourceFileDiameter * 0.5;
file_area = padded_file_radius * padded_file_radius * PI;
visible_count = 0;
visible = false;
position_initialized = false;
since_node_visible = 0.0;
since_last_file_change = 0.0;
since_last_node_change = 0.0;
calcRadius();
calcColour();
}
void RDirNode::fileUpdated(bool userInitiated) {
calcRadius();
if(userInitiated) since_last_file_change = 0.0;
nodeUpdated(userInitiated);
}
void RDirNode::nodeUpdated(bool userInitiated) {
if(userInitiated) since_last_node_change = 0.0;
calcRadius();
updateFilePositions();
if(parent !=0) parent->nodeUpdated(true);
}
void RDirNode::nodeUpdated(bool userInitiated) {
if(userInitiated) since_last_node_change = 0.0;
calcRadius();
updateFilePositions();
if(visible && noDirs() && noFiles()) visible = false;
if(parent !=0) parent->nodeUpdated(true);
}
polar_t convertRectangularToPolar( complex_t c )
{
polar_t p;
p.radius = calcRadius( c );
p.angle = calcAngle( c );
return p;
}
Array_c* power_old( Array_c* vec ) {
Array_c* power_spectrum = newArray_c( vec->length );
double radius;
for( int i = 0; i < vec->length; i++ ) {
radius = calcRadius( vec->a[i] );
power_spectrum->a[i].re = radius * radius;
power_spectrum->a[i].im = 0;
}
return power_spectrum;
}
//generate the locus of pure pursuit
static std::vector<geometry_msgs::Point> generateTrajectoryCircle(geometry_msgs::Point target)
{
std::vector<geometry_msgs::Point> traj_circle_array;
double radius = calcRadius(target);
for (double i = 0; i < M_PI / 2; i += 0.1)
{
//calc a point of circumference
geometry_msgs::Point p;
p.x = radius * cos(i);
p.y = radius * sin(i);
//transform to (radius,0)
geometry_msgs::Point relative_p;
relative_p.x = p.x - radius;
relative_p.y = p.y;
//rotate -90°
geometry_msgs::Point rotate_p = rotatePoint(relative_p,-90);
//transform to vehicle plane
geometry_msgs::Point tf_p = calcAbsoluteCoordinate(rotate_p, _current_pose.pose);
traj_circle_array.push_back(tf_p);
}
//reverse vector
std::reverse(traj_circle_array.begin(), traj_circle_array.end());
for (double i = 0; i > (-1) * M_PI / 2; i -= 0.1)
{
//calc a point of circumference
geometry_msgs::Point p;
p.x = radius * cos(i);
p.y = radius * sin(i);
//transform to (radius,0)
geometry_msgs::Point relative_p;
relative_p.x = p.x - radius;
relative_p.y = p.y;
//rotate -90°
geometry_msgs::Point rotate_p = rotatePoint(relative_p,-90);
//transform to vehicle plane
geometry_msgs::Point tf_p = calcAbsoluteCoordinate(rotate_p, _current_pose.pose);
traj_circle_array.push_back(tf_p);
}
return traj_circle_array;
}
void GmlModel::calcModelData()
{
myData.clear();
for( int i = 0; i < myStrokes.size(); i++)
{
modelData m;
myData.push_back(m);
}
calcMaxLen(); // calculate min and max values for rendering nicely
calcVectors(); // calculate normal vectors from one point to next
calcRadius(); // calculate radius at each point
calcRPoints(); // calculate points defining a circle at each point, rotate in direction of next to create extrusion
calcEndPoints();
calcTriangles();
}
RDirNode::RDirNode(RDirNode* parent, const std::string & abspath) {
changePath(abspath);
parent = 0;
setParent(parent);
accel = spos = prev_accel = vel = vec2(0.0f);
//NOTE: parent is always being set to 0 so this never gets called ...
//figure out starting position
if(parent !=0) {
vec2 parentPos = parent->getPos();
vec2 offset;
pos = parentPos;
} else {
pos = vec2(0.0f, 0.0f);
}
float padded_file_radius = gGourceFileDiameter * 0.5;
file_area = padded_file_radius * padded_file_radius * PI;
visible_count = 0;
visible = false;
position_initialized = false;
since_node_visible = 0.0;
since_last_file_change = 0.0;
since_last_node_change = 0.0;
calcRadius();
calcColour();
}
BoundingSphere::BoundingSphere(std::vector<vector3> list)
{
vertexList = list;
calcRadius();
}
/**
* @brief Returns cell radius.
*
* @return
*/
units::Length getRadius() const noexcept
{
return calcRadius(getVolume());
}
void Gps::update()
{
float angle = 0.0; /* 4msecごとの車体角度の変化量 (度) */
float radius = 0.0; /* 4msecごとの車体の描く円の半径 */
float el = 0.0; /* 4msec間のエンコーダー値の変化量(left) */
float er = 0.0; /* 4msec間のエンコーダー値の変化量(right) */
float currEl = 0.0; /* 現在のエンコーダー値(left) */
float currEr = 0.0; /* 現在のエンコーダー値(right) */
timeCounter++;
/********* 値取得 *********/
/* エンコーダー値を取得 */
motorL.setCount();
motorR.setCount();
currEl = motorL.getCount();
currEr = motorR.getCount();
/* 4msec間のエンコーダー値の変化量を計算 */
el = currEl - prevEl;
er = currEr - prevEr;
/********* 値取得終了 *********/
/********* 座標計算 ***********/
angle = calcAngle(el, er); /* 曲がった角度を計算 */
radius = calcRadius(el, angle); /* 走行体の描く円の半径を計算 */
calcCoordinates(angle, radius, el); /* 座標の更新 */
calcDirection(angle); /* 現在向いている方向の更新 */
/********* 座標計算終了 *******/
/******* prevEl,Erの更新 ******/
prevEl = currEl;
prevEr = currEr;
/**** prevEl,Erの更新の終了****/
/************ 座標補正 *****************/
//lcd.clear();
//lcd.putf("d", );
//lcd .disp();
/*
if(timeCounter%50 !=0)
{
xHistory += mXCoordinate;
yHistory += mYCoordinate;
mDirectionHistory += mDirection;
}
else
{
xHistory = (xHistory+mXCoordinate)/timeCounter;
yHistory = (yHistory + mYCoordinate)/timeCounter;
mDirectionHistory = (mDirectionHistory + mDirection)/timeCounter;
if(!gAngleTraceFlag)
adjustPosition(xHistory, yHistory, mDirectionHistory);
xHistory = 0;
yHistory = 0;
mDirectionHistory = 0;
timeCounter = 0;
}
*/
prevEl = currEl;
prevEr = currEr;
}
