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(); } }