Exemplo n.º 1
0
SimpleMatrix Quaternion::getRotationMatrix() const 
{
	Quaternion tmp = *this;
	tmp.normalize();						// first normalize
											// matrix entries
	double vxSquare = 2 * tmp.v.x * tmp.v.x;
	double vySquare = 2 * tmp.v.y * tmp.v.y;
	double vzSquare = 2 * tmp.v.z * tmp.v.z;
	double vx_vy = 2 * tmp.v.x * tmp.v.y;
	double vx_vz = 2 * tmp.v.x * tmp.v.z;
	double vy_vz = 2 * tmp.v.y * tmp.v.z;
	double v_x = 2 * s * tmp.v.x;
	double v_y = 2 * s * tmp.v.y;
	double v_z = 2 * s * tmp.v.z;

	SimpleMatrix mat;						// generate the matrix
	mat.at(0,0) = 1 - vySquare -  vzSquare;
	mat.at(0,1) = vx_vy + v_z;
	mat.at(0,2) = vx_vz - v_y;
	mat.at(1,0) = vx_vy - v_z; 
	mat.at(1,1) = 1 - vxSquare - vzSquare;
	mat.at(1,2) = vy_vz + v_x;
	mat.at(2,0) = vx_vz + v_y;
	mat.at(2,1) = vy_vz - v_x;
	mat.at(2,2) = 1 - vxSquare - vySquare;

	return mat;
}
Exemplo n.º 2
0
	// ----------------------------------------------------------------------
	void 
		LocalizationDLSModule::
		estimate_position( void ) throw(){

			shawn::Vec* est_pos;
			double distance_r1,distance_r2;
			int count =0;
			distance_r1 = owner().estimate_distance(node(), *linearization_tool_);
			distance_r1 *=distance_r1;
			NeighborInfoList neighbors;
			collect_neighbors( neighborhood(), lat_anchors, neighbors );

			for( std::vector<shawn::Node*>::iterator iter = beacons_->begin(); iter!=beacons_->end();iter++,count++){
				for( ConstNeighborInfoListIterator iter1 = neighbors.begin(); iter1!=neighbors.end(); iter1++){
					if((*iter)->id() == (*iter1)->node().id() ){
						distance_r2 = (*iter1)->distance();
						//distance_r2 = owner().estimate_distance(node(), **iter);

						if(distance_r2 == std::numeric_limits<double>::max() ||distance_r2 == std::numeric_limits<double>::min())
							vector_r_->at(count,0) = 0;
						else    
							vector_r_->at(count,0) = 0.5*( distance_r1 - (distance_r2*distance_r2) + (vector_r_->at(count,0)));
						break;
					}
				}		
			}	
			SimpleMatrix<double>*  temp = new SimpleMatrix<double>(3,1);	
			*temp= *matrix_a_ * *vector_r_;
			est_pos = new shawn::Vec(temp->at(0,0),temp->at(1,0),temp->at(2,0));
			*est_pos += (linearization_tool_->has_est_position())?
				(linearization_tool_->est_position()):(linearization_tool_->real_position());
			node_w().set_est_position( *est_pos );
			delete temp;
	}