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