int main() { int s1, s2, p, q, len1, len2; int x1, y1, x2, y2; while ( scanf( "%d %d", &p, &q ) ) { if( p == -1 && q == -1 ) break; if ( p != 0 ) { s1 = start( p, &len1 ); cartesian( &x1, &y1, len1, s1, p ); } else x1 = y1 = 0; if ( q != 0 ) { s2 = start( q, &len2 ); cartesian( &x2, &y2, len2, s2, q ); } else x2 = y2 = 0; //printf( "RESULT\n%d %d\n%d %d\n\n", x1, y1, x2, y2 ); printf( "%.2lf\n", sqrt( (double)( ( x1 - x2 ) * ( x1 - x2 ) + ( y1 - y2 ) * ( y1 - y2 ) ) ) ); } return 0; }
mat dynamics::getOrthoXYZ_frame() { mat cartesian(3, 3); for (int i = 1; i <= 3; i++) { for (int j = 1; j <= 3; j++) { if (i == j) cartesian(i, j) = 1; else cartesian(i, j) = 0;// REDUNDANT , USED FOR CLARITY } } return cartesian; }
void camera_controller::handle_mouse_move_event(const os::mouse_move_event &event) { if (event.get_mouse_move_button_state() & os::mouse_move_event::MOUSE_BUTTON_STATE_LEFT) { // Rotate view. math::vec2 mouse_pos_relative = get_normalized_mouse_pos(event.get_mouse_move_pos()); auto new_orientation = spherical_angles_from_mouse_pos(mouse_pos_relative); auto camera_vector = radius * new_orientation.cartesian(); target.set_position(target.get_look_at() - camera_vector); } }
// create a Cartesian image suitable for texture mapping from the raw // bearing/range measurements; also return a mask of valid image regions shared_ptr<DidsonCartesian> Didson::getCartesian(int width, int widthTmp) const { // generate map for Cartesian image vector<int> map; int height; pair<vector<int>, int> tmp1 = createMapping(width, maxRange(), minRange(), consts.bearingFov * 0.5, numBearings(), numRanges()); map = tmp1.first; height = tmp1.second; // avoid having to write out the inverse mapping function by creating // a map with sufficiently high resolution as a lookup table for the inverse map // not ideal, but works... vector<int> invMap(consts.numRanges * consts.numBearings); vector<int> mapTmp; int heightTmp; pair<vector<int>, int> tmp2 = createMapping(widthTmp, maxRange(), minRange(), consts.bearingFov * 0.5, numBearings(), consts.numRanges); mapTmp = tmp2.first; heightTmp = tmp2.second; int c = 0; for (int y = 0; y < heightTmp; y++) { for (int x = 0; x < widthTmp; x++) { int idx = mapTmp[c]; if (idx != -1) { int icol = x * ((double) width / (double) widthTmp); int irow = y * ((double) height / (double) heightTmp); int i = irow * width + icol; invMap[idx] = i; } c++; } } shared_ptr<DidsonCartesian> cartesian(new DidsonCartesian(map, invMap)); cartesian->image = cv::Mat(height, width, CV_8UC1); cartesian->mask = cv::Mat(height, width, CV_8UC1); for (int i = 0; i < width * height; i++) { if (map[i] == -1) { cartesian->image.data[i] = 0; cartesian->mask.data[i] = 0; } else { cartesian->image.data[i] = _image.data[map[i]]; cartesian->mask.data[i] = 255; } } return cartesian; }
void RobotCenter::cycle(){ float mat[3][3]={0}; float mat1[2][3]={0}; if(millis()-time>=5){ time=millis(); for(int i=0;i<3;i++){ gapEncDis[i]=enc[i]->value()-oldEncDisi[i]; oldEncDisi[i]=enc[i]->value(); encAngle[i]=gapEncDis[i]/encRadius; } originalAngle+=(encAngle[0]+encAngle[1]+encAngle[2])/3.0; angle=area(originalAngle,(-1.0)*M_PI,M_PI); for(int i=0;i<3;i++){ moveEncX[i]=fabs(gapEncDis[i])*cos(encMountAngle[i]+angle+radiusReverse(gapEncDis[i])); moveEncY[i]=fabs(gapEncDis[i])*sin(encMountAngle[i]+angle+radiusReverse(gapEncDis[i])); if(tan(encMountAngle[i]+angle)!=0.0)mat[i][0]=1.0/tan(encMountAngle[i]+angle); else mat[i][0]=0.0; mat[i][1]=1.0; mat[i][2]=moveEncY[i]+mat[i][0]*moveEncX[i]; } for(int i=0;i<3;i++){ Ans_t ans; int j=2-(i+1); if(j==-1) j=2; mat1[0][0]=mat[2-i][0]; mat1[0][1]=mat[2-i][1]; mat1[0][2]=mat[2-i][2]; mat1[1][0]=mat[j][0]; mat1[1][1]=mat[j][1]; mat1[1][2]=mat[j][2]; ans=crameRequation(mat1); temporaryX[i]=ans.x; temporaryY[i]=ans.y; } coordX+=(temporaryX[0]+temporaryX[1]+temporaryX[2])/3.0; coordY+=(temporaryY[0]+temporaryY[1]+temporaryY[2])/3.0; } cartesian(coordX,coordY,0); }
/* * converts cartesian via points to joint space via points */ vector<ColumnVector> TrajectoryController::getJSPoints( vector<ColumnVector>& cartesianList) { vector<ColumnVector> jsPoints; ColumnVector cartesian(6), currentQ(6), previousQ(6), joint(6); Generator* gen = getGenerator(cartesianList); if (gen != NULL) { currentQ = 0; joint = 0; // - convert ith cartesian to thetas // - if conditions satisfied, add point in path. for (int i = 0; i <= PNT_SAMPLES; i++) { double k = ((i * 1.0) / PNT_SAMPLES); // cout << k << endl; //get xyz and orientation from generator for time k. cartesian = gen->getPosition(k); // cout << "cart c " << cartesian.as_row(); Quaternion quat = gen->getOrientation(k); cartesian.resize_keep(6); Matrix R = quat.R(); cartesian.Rows(4, 6) = irpy(R); // cout << cartesian.AsRow() << endl; // cout << quat << endl; //add the joint solution if it exists if (kineSolver->getBestSolution(cartesian, currentQ, joint)) { // cout << "cartesian " << cartesian.AsRow() << endl; // cout << "joint " << joint.AsRow() << endl; jsPoints.push_back(joint); currentQ = joint; } else { //path has to be rejected cout << cartesian.AsRow() << " : Via point out of workspace. Path rejected." << endl; jsPoints.clear(); break; } } } return jsPoints; }
//TODO: ReturnMatrix KinematicsSolver::getCartesianPos(ColumnVector& currentQ) { ColumnVector cartesian(6); cartesian = 0; Matrix Rot; ColumnVector p; robot->set_q(currentQ); robot->kine(Rot, p); cartesian.Rows(1, 3) = p; cartesian.Rows(4, 6) = irpy(Rot);//this->getAnglesFromRotationMatrix(Rot); // cout << "bc " << cartesian.AsRow(); // // cartesian(4) = normalizeAngle(cartesian(4)); // cartesian(5) = normalizeAngle(cartesian(5)); // cartesian(6) = normalizeAngle(cartesian(6)); // // cout << "ad " << cartesian.AsRow() << endl; cartesian.Release(); return cartesian; }
//Rendering void myApp::renderInternal() { D3DXMATRIX *M = new D3DXMATRIX; m_nClearColor = 0x00000000; m_pD3D->clear(m_nClearColor); //Coordinate system rendering float const axisLength = 10.0f; vertex const system[6] = { { cartesian(0, 0, 0), 0xFFFF0000 }, { cartesian(axisLength, 0, 0), 0xFFFF0000 }, { cartesian(0, 0, 0), 0xFFFF0000 }, { cartesian(0, axisLength, 0), 0xFFFFFF00 }, { cartesian(0, 0, 0), 0xFFFF0000 }, { cartesian(0, 0, axisLength), 0xFFFF0000 }, }; m_pD3D->getDevice()->SetTransform(D3DTS_WORLD, D3DXMatrixIdentity(M)); m_pD3D->getDevice()->DrawPrimitiveUP(D3DPT_LINELIST, 3, system, sizeof(vertex)); delete M; //Object rendering generator.render(); }
/** Kernel L2P operation * r += Op(L, t) where L is the local expansion and r is the result * * @param[in] L The local expansion * @param[in] center The center of the box with the local expansion * @param[in] target The target of this L2P operation * @param[in] result The result to accumulate into * @pre L includes the influence of all sources outside its box */ void L2P(const local_type& L, const point_type& center, const target_type& target, result_type& result) const { complex Ynm[4*P*P], YnmTheta[4*P*P]; point_type dist = target - center; point_type gradient[4]; // = {0.,0.,0.,0.}; gradient[0] = point_type(0.); gradient[1] = point_type(0.); gradient[2] = point_type(0.); gradient[3] = point_type(0.); point_type cartesian(0); real r, theta, phi; cart2sph(r,theta,phi,dist); evalMultipole(r,theta,phi,Ynm,YnmTheta); #ifdef STRESSLET double scale = 1./6; #else double scale = 1.; #endif for( int n=0; n!=P; ++n ) { int nm = n * n + n; int nms = n * (n + 1) / 2; result[0] += scale*std::real(L[0][nms] * Ynm[nm]); result[1] += scale*std::real(L[1][nms] * Ynm[nm]); result[2] += scale*std::real(L[2][nms] * Ynm[nm]); real factor = 1. / r * n; gradient[0][0] += std::real(L[0][nms] * Ynm[nm]) * factor; gradient[0][1] += std::real(L[0][nms] * YnmTheta[nm]); gradient[1][0] += std::real(L[1][nms] * Ynm[nm]) * factor; gradient[1][1] += std::real(L[1][nms] * YnmTheta[nm]); gradient[2][0] += std::real(L[2][nms] * Ynm[nm]) * factor; gradient[2][1] += std::real(L[2][nms] * YnmTheta[nm]); gradient[3][0] += std::real(L[3][nms] * Ynm[nm]) * factor; gradient[3][1] += std::real(L[3][nms] * YnmTheta[nm]); for( int m=1; m<=n; ++m ) { nm = n * n + n + m; nms = n * (n + 1) / 2 + m; result[0] += scale * 2 * std::real(L[0][nms] * Ynm[nm]); result[1] += scale * 2 * std::real(L[1][nms] * Ynm[nm]); result[2] += scale * 2 * std::real(L[2][nms] * Ynm[nm]); gradient[0][0] += 2 * std::real(L[0][nms] * Ynm[nm]) * factor; gradient[0][1] += 2 * std::real(L[0][nms] * YnmTheta[nm]); gradient[0][2] += 2 * std::real(L[0][nms] * Ynm[nm] * CI) * m; gradient[1][0] += 2 * std::real(L[1][nms] * Ynm[nm]) * factor; gradient[1][1] += 2 * std::real(L[1][nms] * YnmTheta[nm]); gradient[1][2] += 2 * std::real(L[1][nms] * Ynm[nm] * CI) * m; gradient[2][0] += 2 * std::real(L[2][nms] * Ynm[nm]) * factor; gradient[2][1] += 2 * std::real(L[2][nms] * YnmTheta[nm]); gradient[2][2] += 2 * std::real(L[2][nms] * Ynm[nm] * CI) * m; gradient[3][0] += 2 * std::real(L[3][nms] * Ynm[nm]) * factor; gradient[3][1] += 2 * std::real(L[3][nms] * YnmTheta[nm]); gradient[3][2] += 2 * std::real(L[3][nms] * Ynm[nm] * CI) * m; } } sph2cart(r,theta,phi,gradient[0],cartesian); cartesian *= -target[0]; gradient[0] = cartesian; sph2cart(r,theta,phi,gradient[1],cartesian); cartesian *= -target[1]; gradient[1] = cartesian; sph2cart(r,theta,phi,gradient[2],cartesian); cartesian *= -target[2]; gradient[2] = cartesian; sph2cart(r,theta,phi,gradient[3],cartesian); gradient[3] = cartesian; result[0] += scale*(gradient[0][0]+gradient[1][0]+gradient[2][0]+gradient[3][0]); result[1] += scale*(gradient[0][1]+gradient[1][1]+gradient[2][1]+gradient[3][1]); result[2] += scale*(gradient[0][2]+gradient[1][2]+gradient[2][2]+gradient[3][2]); }
AwPoint AwPoint::cartesianSub(const AwVector &v) const { AwPoint pt = cartesian(); return AwPoint(pt.x - v.x, pt.y - v.y, pt.z - v.z); }
AwPoint AwPoint::cartesianAdd(const AwVector &v) const { AwPoint pt = cartesian(); return AwPoint(pt.x + v.x, pt.y + v.y, pt.z + v.z); }
AwVector AwPoint::cartesianSub(const AwPoint &otherPt) const { AwPoint ptA = cartesian(); AwPoint ptB = otherPt.cartesian(); return AwVector(ptA.x - ptB.x, ptA.y - ptB.y, ptA.z - ptB.z); }
static pose from_vector (const vector_type& v) { return cartesian (v(0), v(1), v(2)); }