void cDrawSimCharacter::DrawCharFeatures(const cSimCharacter& character, const cGround& ground, double marker_size, double vel_scale,
const tVector& pos_col, const tVector& vel_col, const tVector& offset)
{
const double arrow_size = marker_size * 0.65;
tVector root_pos = character.GetRootPos();
double ground_h = ground.SampleHeight(root_pos);
tVector ground_pos = root_pos;
ground_pos[1] = ground_h;
cDrawUtil::SetColor(tVector(pos_col[0], pos_col[1], pos_col[2], pos_col[3]));
cDrawUtil::DrawArrow2D(ground_pos + offset, root_pos + offset, arrow_size);
for (int i = 0; i < character.GetNumBodyParts(); ++i)
{
if (character.IsValidBodyPart(i))
{
const auto& curr_part = character.GetBodyPart(i);
tVector pos = curr_part->GetPos();
tVector vel = curr_part->GetLinearVelocity();
cDrawUtil::SetColor(tVector(pos_col[0], pos_col[1], pos_col[2], pos_col[3]));
cDrawUtil::DrawArrow2D(root_pos + offset, pos + offset, arrow_size);
cDrawUtil::SetColor(tVector(vel_col[0], vel_col[1], vel_col[2], vel_col[3]));
cDrawUtil::DrawArrow2D(pos + offset, pos + vel * vel_scale + offset, arrow_size);
}
}
}
tReal tPropSurface::hubRadiusAt(tReal /*x*/)
{
// Spaeter mal durch interpolation eines Splines ersetzen.
// return hubDiameter*0.5*fDiameter*fScale;
tVector X;
X = vectorAt(0,0);
X = X.toCylCOS(tVector(0,0,0),tVector(1,0,0),tVector(0,1,0));
return X.y;
}
tVector cMathUtil::InvEuler(const tVector& euler)
{
tMatrix inv_mat = cMathUtil::RotateMat(tVector(1, 0, 0, 0), -euler[0])
* cMathUtil::RotateMat(tVector(0, 1, 0, 0), -euler[1])
* cMathUtil::RotateMat(tVector(0, 0, 1, 0), -euler[2]);
tVector inv_euler = cMathUtil::RotMatToEulerAngles(inv_mat);
tMatrix test_mat = cMathUtil::RotateMat(euler);
tMatrix I = inv_mat * test_mat;
return inv_euler;
}
void cDrawScenarioImitateStep::DrawStepPlan() const
{
const tVector& pos0_col = tVector(0, 1, 0, 0.5);
const tVector& pos1_col = tVector(0, 0.5, 0, 0.5);
const tVector& heading_col = tVector(1, 0, 0, 0.5);
auto step_scene = std::dynamic_pointer_cast<cScenarioExpImitateStep>(mScene);
const auto& step_plan = step_scene->GetStepPlan();
DrawStepPos(step_plan.mStepPos0, pos0_col);
DrawStepPos(step_plan.mStepPos1, pos1_col);
DrawRootHeading(step_plan, heading_col);
}
void cDrawScenarioRegVar1D::DrawNetEval() const
{
cDrawScenarioReg1D::DrawNetEval();
const tVector col0 = tVector(0, 0, 1, 0.75);
const tVector col1 = tVector(0, 0.5, 0, 0.75);
const double weight0 = 3;
const double weight1 = 2;
const int dash_skip = 4;
const auto& eval_pts = mScene->GetEvalPts();
int num_pts = static_cast<int>(eval_pts.size());
for (int i = 0; i < (num_pts - 1); ++i)
{
const tVector& a = eval_pts[i];
const tVector& b = eval_pts[i + 1];
cDrawUtil::SetLineWidth(weight0);
cDrawUtil::SetColor(col0);
cDrawUtil::DrawLine(tVector(a[0], a[1], 0, 0), tVector(b[0], b[1], 0, 0));
if ((i % dash_skip) <= (dash_skip / 2))
{
cDrawUtil::SetLineWidth(weight1);
cDrawUtil::SetColor(col1);
cDrawUtil::DrawLine(tVector(a[0], a[1] + 2 * a[2], 0, 0), tVector(b[0], b[1] + 2 * b[2], 0, 0));
cDrawUtil::DrawLine(tVector(a[0], a[1] - 2 * a[2], 0, 0), tVector(b[0], b[1] - 2 * b[2], 0, 0));
}
}
}
void cDrawSimCharacter::DrawSimBody(const cSimCharacter& character, const tVector& fill_tint, const tVector& line_col)
{
const tVector gContactCol = tVector(0.5, 0.75, 0.5, 1);
cDrawUtil::SetLineWidth(1);
for (int i = 0; i < character.GetNumBodyParts(); ++i)
{
if (character.IsValidBodyPart(i))
{
const std::shared_ptr<cSimBox>& curr_part = std::static_pointer_cast<cSimBox>(character.GetBodyPart(i));
tVector pos = curr_part->GetPos();
tVector col;
if (curr_part->IsInContact())
{
col = gContactCol;
}
else
{
col = character.GetPartColor(i);
col = col.cwiseProduct(fill_tint);
}
cDrawUtil::SetColor(col);
cDrawObj::Draw(curr_part.get(), cDrawUtil::eDrawSolid);
if (line_col[3] > 0)
{
cDrawUtil::SetColor(line_col);
cDrawObj::Draw(curr_part.get(), cDrawUtil::eDrawWire);
}
}
}
}
CSkeleton::ESkeletonStates CSkeleton::Manuver()
{
XMFLOAT3 tVector(0.0f, m_f3Velocity.y, 0.0f);
// Update the timer
ResetManuverVector(m_nChance, tVector);
m_fExitTimer -= DELTA_TIME();
if (m_fExitTimer < 0.0f)
{
m_nManuverCount++;
m_fExitTimer = 1.5f;
if (m_nManuverCount == m_nManuverCount)
return MOVING_IN;
}
Steering().Seek(*GetPlayer()->GetPosition());
if (m_bIsGrounded)
{
Steering().Update(false, m_fScale);
SetWorldVelocity(tVector);
}
else
SetWorldVelocity({ knockBackVelocity.x, m_f3Velocity.y, knockBackVelocity.z });
return MANUVERING;
}
Eigen::VectorXd cIKSolver::BuildConsPosErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const tConsDesc& cons_desc, double clamp_dist)
{
assert(static_cast<int>(cons_desc(eConsDescType)) == eConsTypePos);
int parent_id = static_cast<int>(cons_desc(eConsDescParam0));
tVector attach_pt = tVector(cons_desc(eConsDescParam1), cons_desc(eConsDescParam2), 0.f, 0.f);
tVector target_pos = tVector(cons_desc(eConsDescParam3), cons_desc(eConsDescParam4), 0.f, 0.f);
tVector end_pos = cKinTree::LocalToWorldPos(joint_mat, pose, parent_id, attach_pt);
tVector delta = target_pos - end_pos;
ClampMag(delta, clamp_dist);
Eigen::Vector2d err = Eigen::Vector2d(delta[0], delta[1]);
return err;
}
void cMathUtil::EulerToAxisAngle(const tVector& euler, tVector& out_axis, double& out_theta)
{
double x = euler[0];
double y = euler[1];
double z = euler[2];
double x_s = std::sin(x);
double x_c = std::cos(x);
double y_s = std::sin(y);
double y_c = std::cos(y);
double z_s = std::sin(z);
double z_c = std::cos(z);
double c = (y_c * z_c + x_s * y_s * z_s + x_c * z_c + x_c * y_c - 1) * 0.5;
c = Clamp(c, -1.0, 1.0);
out_theta = std::acos(c);
if (std::abs(out_theta) < 0.00001)
{
out_axis = tVector(0, 0, 1, 0);
}
else
{
double m21 = x_s * y_c - x_c * y_s * z_s + x_s * z_c;
double m02 = x_c * y_s * z_c + x_s * z_s + y_s;
double m10 = y_c * z_s - x_s * y_s * z_c + x_c * z_s;
double denom = std::sqrt(m21 * m21 + m02 * m02 + m10 * m10);
out_axis[0] = m21 / denom;
out_axis[1] = m02 / denom;
out_axis[2] = m10 / denom;
out_axis[3] = 0;
}
}
tTransformedDotListCurve::tTransformedDotListCurve(void *theOwner): tDotListCurve(theOwner)
{
shift_ = tVector (0,0,0);
axis_ = NULL;
alpha_ = 0.;
dlc_ = NULL;
}
// Returns a rotated vector A, according the matrix
tVector tMatrix::Rotate ( const tVector &A) const
{
float _x = A.getX() * c + A.getY() *-s;
float _y = A.getX() * s + A.getY() * c;
return tVector( _x, _y);
}
void cDrawScenarioSimInteractive::MouseClick(int button, int state, double x, double y)
{
const double ray_max_dist = 1000;
cDrawScenarioTerrainRL::MouseClick(button, state, x, y);
if (button == GLUT_LEFT_BUTTON)
{
if (state == GLUT_DOWN)
{
mClickScreenPos = tVector(x, y, 0, 0);
mDragScreenPos = mClickScreenPos;
tVector start = mCam.ScreenToWorldPos(mClickScreenPos);
tVector dir = mCam.GetRayCastDir(start);
tVector end = start + dir * ray_max_dist;
cWorld::tRayTestResult raytest_result;
RayTest(start, end, raytest_result);
HandleRayTest(raytest_result);
}
else if (state == GLUT_UP)
{
ResetUI();
}
}
}
Eigen::MatrixXd cIKSolver::BuildConsPosJacob(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const tConsDesc& cons_desc)
{
assert(static_cast<int>(cons_desc(eConsDescType)) == eConsTypePos);
int num_joints = static_cast<int>(joint_mat.rows());
int parent_id = static_cast<int>(cons_desc(eConsDescParam0));
tVector attach_pt = tVector(cons_desc(eConsDescParam1), cons_desc(eConsDescParam2), 0.f, 0.f);
tVector end_pos = cKinTree::LocalToWorldPos(joint_mat, pose, parent_id, attach_pt);
const Eigen::Vector3d rot_axis = Eigen::Vector3d(0, 0, 1);
int num_dof = cKinTree::GetNumDof(joint_mat);
Eigen::MatrixXd J = Eigen::MatrixXd(gPosDims, num_dof);
J.setZero();
for (int i = 0; i < gPosDims; ++i)
{
J(i, i) = 1;
}
int curr_id = parent_id;
while (true)
{
tVector joint_pos = cKinTree::CalcJointWorldPos(joint_mat, pose, curr_id);
tVector delta = end_pos - joint_pos;
Eigen::Vector3d tangent = rot_axis.cross(Eigen::Vector3d(delta(0), delta(1), delta(2)));
int curr_parent_id = cKinTree::GetParent(joint_mat, curr_id);
for (int i = 0; i < gPosDims; ++i)
{
J(i, gPosDims + curr_id) = tangent(i);
}
if (curr_parent_id == cKinTree::gInvalidJointID)
{
// no scaling for root
break;
}
else
{
#if !defined(DISABLE_LINK_SCALE)
double attach_x = joint_desc(curr_id, cKinTree::eJointDescAttachX);
double attach_y = joint_desc(curr_id, cKinTree::eJointDescAttachY);
double attach_z = joint_desc(curr_id, cKinTree::eJointDescAttachZ);
double parent_world_theta = cKinTree::CalcJointWorldTheta(joint_desc, curr_parent_id);
double world_attach_x = std::cos(parent_world_theta) * attach_x - std::sin(parent_world_theta) * attach_y;
double world_attach_y = std::sin(parent_world_theta) * attach_x + std::cos(parent_world_theta) * attach_y;
double world_attach_z = attach_z; // hack ignoring z, do this properly
J(0, gPosDims + num_joints + curr_id) = world_attach_x;
J(1, gPosDims + num_joints + curr_id) = world_attach_y;
#endif
curr_id = curr_parent_id;
}
}
return J;
}
cMeshUtil::tVertex cMeshUtil::GetVertex(int v, const cDrawMesh& mesh)
{
const float* pos_data = mesh.GetData(eAttributePosition);
const float* norm_data = mesh.GetData(eAttributeNormal);
const float* coord_data = mesh.GetData(eAttributeCoord);
int pos_dim = gPosDim;
int norm_dim = gNormDim;
int coord_dim = gCoordDim;
tVertex vert;
vert.mPosition = tVector(pos_data[v * pos_dim], pos_data[v * pos_dim + 1], pos_data[v * pos_dim + 2], 0);
vert.mNormal = tVector(norm_data[v * norm_dim], norm_data[v * norm_dim + 1], norm_data[v * norm_dim + 2], 0);
vert.mCoord = Eigen::Vector2d(coord_data[v * coord_dim], coord_data[v * coord_dim + 1]);
return vert;
}
void cMathUtil::QuaternionToAxisAngle(const tQuaternion& q, tVector& out_axis, double& out_theta)
{
out_theta = 0;
out_axis = tVector(0, 0, 1, 0);
tQuaternion q1 = q;
if (std::abs(q1.w()) > 1)
{
q1.normalize();
}
double sin_theta = q.vec().norm();
if (sin_theta > 0.0001)
{
out_theta = 2 * cMathUtil::Sign(q.w()) * std::asin(sin_theta);
out_axis = tVector(q.x(), q.y(), q.z(), 0) / sin_theta;
}
}
void cDrawScenarioSimInteractive::MouseMove(double x, double y)
{
cDrawScenarioTerrainRL::MouseMove(x, y);
if (ObjectSelected())
{
mDragScreenPos = tVector(x, y, 0, 0);
}
}
/*
* Returns a vector clamped by this BoundingBox
*/
tVector tBoundingBox::Clamp( const tVector &vec)
{
float x = vec.getX();
float y = vec.getY();
x = (x > _max.getX()) ? _max.getX() : (x < _min.getX()) ? _min.getX() : x;
y = (y > _max.getY()) ? _max.getY() : (y < _min.getY()) ? _min.getY() : y;
return tVector( x, y);
}
// Passes reference of "msg", is used as output
// Executes the SegmentStart sub-controller based on the rest of the arguments
enum SUBRETURN cSegmentStart::Execute( tVector &output, const flarb_msgs::VDState &state, cMap &map)
{
// First check if there ain't room at both sides
tBoundingBox a( tVector( -0.5f, 0), tVector( 0.0f, 0.1f));
tBoundingBox b( tVector( 0.0f, 0), tVector( 0.5f, 0.1f));
if( map.CheckIntersectionRegion( a) && map.CheckIntersectionRegion( b))
{
return RET_NEXT;
}
tVector direction = tVector( 0.0f, _ParamCheckRange);
map.FindFreePath( FLARB_EXTRA_RADIUS, direction, output, false);
if(output.Length() > _ParamSpeed)
output.setLength( _ParamSpeed);
return RET_SUCCESS;
}
void cDrawScenarioImitateStep::DrawRootHeading(const cBipedStepController3D::tStepPlan& step_plan, const tVector& col) const
{
const tVector offset = tVector(0, 0.02, 0, 0);
const double arrow_size = 0.2;
const double arrow_len = 0.5;
double theta = step_plan.mRootHeading;
tVector start = 0.5 * (step_plan.mStepPos0 + step_plan.mStepPos1);
start += offset;
tVector dir = tVector(std::cos(theta), std::sin(theta), 0, 0);
tVector end = arrow_len * dir;
cDrawUtil::SetColor(col);
cDrawUtil::PushMatrix();
cDrawUtil::Translate(start);
cDrawUtil::Rotate(-0.5 * M_PI, tVector(1, 0, 0, 0));
cDrawUtil::DrawArrow2D(tVector::Zero(), end, arrow_size);
cDrawUtil::PopMatrix();
}
void cDrawSimCharacter::DrawCoM(const cSimCharacter& character, double marker_size, double vel_scale,
const tVector& col, const tVector& offset)
{
const double arrow_size = marker_size * 0.65;
tVector com = character.CalcCOM();
tVector com_vel = character.CalcCOMVel();
cDrawUtil::SetLineWidth(4);
cDrawUtil::SetColor(tVector(col[0], col[1], col[2], col[3]));
cDrawUtil::DrawCross(com + offset, marker_size);
cDrawUtil::DrawArrow2D(com + offset, com + offset + com_vel * vel_scale, arrow_size);
}
tMatrix cMathUtil::DirToRotMat(const tVector& dir, const tVector& up)
{
tVector x = up.cross3(dir);
double x_norm = x.norm();
if (x_norm == 0)
{
x_norm = 1;
x = (dir.dot(up) >= 0) ? tVector(1, 0, 0, 0) : tVector(-1, 0, 0, 0);
}
x /= x_norm;
tVector y = dir.cross3(x).normalized();
tVector z = dir;
tMatrix mat = tMatrix::Identity();
mat.block(0, 0, 3, 1) = x.segment(0, 3);
mat.block(0, 1, 3, 1) = y.segment(0, 3);
mat.block(0, 2, 3, 1) = z.segment(0, 3);
return mat;
}
void cDrawGround::DrawFlat3D(const cGround* ground, const tVector& col, const tVector& bound_min, const tVector& bound_max)
{
assert(ground->GetGroundType() == cGround::eGroundTypeFlat);
const cGroundFlat* ground_flat = reinterpret_cast<const cGroundFlat*>(ground);
tVector pos = ground_flat->GetPos();
tVector size = bound_max - bound_min;
cDrawUtil::SetColor(col);
glPushMatrix();
cDrawUtil::Rotate(-0.5 * M_PI, tVector(1, 0, 0, 0));
cDrawUtil::DrawRect(pos, size, cDrawUtil::eDrawSolid);
glPopMatrix();
}
void cDrawGround::DrawFlat2D(const cGround* ground, const tVector& col, const tVector& bound_min, const tVector& bound_max)
{
assert(ground->GetGroundType() == cGround::eGroundTypeFlat);
const cGroundFlat* ground_flat = reinterpret_cast<const cGroundFlat*>(ground);
tVector ground_origin = ground_flat->GetPos();
const double ground_h = ground_flat->SampleHeight(tVector::Zero());
tVector origin = (bound_min + bound_max) * 0.5;
double w = bound_max[0] - bound_min[0];
double h = bound_max[1] - bound_min[1];
double max_x = origin(0) + w;
double max_y = ground_h;
double min_x = origin(0) - w;
double min_y = std::min(origin(1) - h * 0.5f, max_y - 0.05f);
tVector pos = tVector(origin(0), (min_y + max_y) * 0.5, 0, 0);
tVector size = tVector(w, (max_y - min_y), 0, 0);
cDrawUtil::DrawRuler2D(pos, size, col, gMarkerSpacing, gBigMarkerSpacing, gMarkerH, gBigMarkerH);
}
void DXFReader::addLine (const DL_LineData &line)
{
tLayer *layer = model->currentLayer();
if (layer){
tLine *l = layer->addLine();
if (l){
l->setTResolution(1);
activeElement = l;
QString type = activeElement->intrface()->type();
type.append(QString::number(getElementCounter(type)));
activeElement->intrface()->setName(type);
subLayer = dynamic_cast<tLayer*>(layer->addElement(iLayer::theType()));
subLayer->intrface()->setName(type+"_vertices");
subLayer->intrface()->setVisibility(false);
tDot *d1 = subLayer->addPoint(tVector(line.x1,line.y1,line.z1)),
*d2 = subLayer->addPoint(tVector(line.x2,line.y2,line.z2));
l->setVertices(d1,d2);
}
}
}
void cJoint::CalcRotation(tVector& out_axis, double& out_theta) const
{
assert(IsValid());
switch (mParams.mType)
{
case cWorld::eJointTypeHinge:
CalcRotationHinge(out_axis, out_theta);
break;
case cWorld::eJointTypePrismatic:
out_axis = tVector(0, 0, 1, 0);
out_theta = CalcDisplacementPrismatic();
break;
default:
printf("Unsupported constraint type for cJoint::CalcRotation()\n");
break;
}
}
void cTerrainGen3D::AddPath(const tVector& origin, const Eigen::Vector2i& start_coord, const tVector& size, const Eigen::Vector2i& out_res,
double w, double len, double turn_len, double h0, double h1,
std::vector<float>& out_data, std::vector<int>& out_flags)
{
int res_x = CalcResX(size[0]);
int res_z = CalcResX(size[2]);
int num_verts = res_x * res_z;
for (int j = 0; j < res_z; ++j)
{
size_t coord_z = j + start_coord[1];
for (int i = 0; i < res_x; ++i)
{
size_t coord_x = i + start_coord[0];
size_t idx = coord_z * out_res[0] + coord_x;
tVector curr_pos = origin + tVector(i * gVertSpacing, 0, j * gVertSpacing, 0);
int curr_flags = 1 << eVertFlagEnableTex;
double curr_h = h1;
int interval = std::floor(curr_pos[0] / len);
if (interval % 2 == 0)
{
curr_h = (std::abs(curr_pos[2]) < 0.5 * w) ? h0 : h1;
}
else
{
curr_h = (std::abs(turn_len - curr_pos[2]) < 0.5 * w) ? h0 : h1;
}
float interval_start = interval * len;
float interval_end = (interval + 1) * len;
if (std::abs(interval_start - curr_pos[0]) < 0.5 * w
|| std::abs(interval_end - curr_pos[0]) < 0.5 * w)
{
if (curr_pos[2] > -0.5 * w
&& curr_pos[2] < turn_len + 0.5 * w)
{
curr_h = h0;
}
}
out_data[idx] = curr_h;
out_flags[idx] = curr_flags;
}
}
}
tVector cMathUtil::CalcBarycentric(const tVector& p, const tVector& a, const tVector& b, const tVector& c)
{
tVector v0 = b - a;
tVector v1 = c - a;
tVector v2 = p - a;
double d00 = v0.dot(v0);
double d01 = v0.dot(v1);
double d11 = v1.dot(v1);
double d20 = v2.dot(v0);
double d21 = v2.dot(v1);
double denom = d00 * d11 - d01 * d01;
double v = (d11 * d20 - d01 * d21) / denom;
double w = (d00 * d21 - d01 * d20) / denom;
double u = 1.0f - v - w;
return tVector(u, v, w, 0);
}
void DXFReader::addControlPoint(const DL_ControlPointData& data)
{
tDxfSpline* spl = dynamic_cast<tDxfSpline*>(activeElement);
if (spl){
tLayer *l;
if (subLayer){
l = subLayer;
} else {
l = model->currentLayer();
}
tPoint *p = l->addPoint(tVector(data.x, data.y, data.z));
spl->addVertex(p);
QString type = p->intrface()->type();
type.append(QString::number(getElementCounter(type)));
p->intrface()->setName(activeElement->intrface()->name() + "_" + type);
}
}
void cDrawSimCharacter::DrawTerainFeatures(const cSimCharacter& character, double marker_size,
const tVector& terrain_col, const tVector& offset)
{
const double arrow_size = marker_size * 0.65;
int num_terrain_features = GetCharNumGroundFeatures(character);
tVector origin = GetCharGroundSampleOrigin(character);
double base_h = origin[1];
cDrawUtil::SetColor(tVector(terrain_col[0], terrain_col[1], terrain_col[2], terrain_col[3]));
for (int i = 0; i < num_terrain_features; ++i)
{
tVector sample = GetCharGroundSample(character, i);
tVector base_sample = sample;
base_sample[1] = base_h;
cDrawUtil::DrawArrow2D(base_sample + offset, sample + offset, arrow_size);
}
}
void cDrawKinTree::DrawTree(const Eigen::MatrixXd& joint_desc, const Eigen::VectorXd& pose, int joint_id, double link_width,
const tVector& fill_col, const tVector& line_col)
{
const double node_radius = 0.02;
if (joint_id != cKinTree::gInvalidJointID)
{
bool has_parent = cKinTree::HasParent(joint_desc, joint_id);
if (has_parent)
{
tVector attach_pt = cKinTree::GetScaledAttachPt(joint_desc, joint_id);
attach_pt[2] = 0; // hack ignore z
double len = attach_pt.norm();
glPushMatrix();
tVector pos = tVector(len / 2, 0, 0, 0);
tVector size = tVector(len, link_width, 0, 0);
double theta = std::atan2(attach_pt[1], attach_pt[0]);
cDrawUtil::Rotate(theta, tVector(0, 0, 1, 0));
cDrawUtil::SetColor(tVector(fill_col[0], fill_col[1], fill_col[2], fill_col[3]));
cDrawUtil::DrawRect(pos, size, cDrawUtil::eDrawSolid);
cDrawUtil::SetColor(tVector(line_col[0], line_col[1], line_col[2], line_col[3]));
cDrawUtil::DrawRect(pos, size, cDrawUtil::eDrawWire);
// draw node
cDrawUtil::SetColor(tVector(fill_col[0] * 0.25, fill_col[1] * 0.25, fill_col[2] * 0.25, fill_col[3]));
cDrawUtil::DrawDisk(node_radius, 16);
glPopMatrix();
}
glPushMatrix();
tMatrix m = cKinTree::ChildParentTrans(joint_desc, pose, joint_id);
cDrawUtil::GLMultMatrix(m);
Eigen::VectorXi children;
cKinTree::FindChildren(joint_desc, joint_id, children);
for (int i = 0; i < children.size(); ++i)
{
int child_id = children[i];
DrawTree(joint_desc, pose, child_id, link_width, fill_col, line_col);
}
glPopMatrix();
}
}