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