//**********************************************************************************
//test of polyhedron intersection callable from python shell
bool do_Polyhedras_Intersect(const shared_ptr<Shape>& cm1,const shared_ptr<Shape>& cm2,const State& state1,const State& state2){
const Se3r& se31=state1.se3;
const Se3r& se32=state2.se3;
Polyhedra* A = static_cast<Polyhedra*>(cm1.get());
Polyhedra* B = static_cast<Polyhedra*>(cm2.get());
//move and rotate 1st the CGAL structure Polyhedron
Matrix3r rot_mat = (se31.orientation).toRotationMatrix();
Vector3r trans_vec = se31.position;
Transformation t_rot_trans(rot_mat(0,0),rot_mat(0,1),rot_mat(0,2), trans_vec[0],rot_mat(1,0),rot_mat(1,1),rot_mat(1,2),trans_vec[1],rot_mat(2,0),rot_mat(2,1),rot_mat(2,2),trans_vec[2],1.);
Polyhedron PA = A->GetPolyhedron();
std::transform( PA.points_begin(), PA.points_end(), PA.points_begin(), t_rot_trans);
//move and rotate 2st the CGAL structure Polyhedron
rot_mat = (se32.orientation).toRotationMatrix();
trans_vec = se32.position;
t_rot_trans = Transformation(rot_mat(0,0),rot_mat(0,1),rot_mat(0,2), trans_vec[0],rot_mat(1,0),rot_mat(1,1),rot_mat(1,2),trans_vec[1],rot_mat(2,0),rot_mat(2,1),rot_mat(2,2),trans_vec[2],1.);
Polyhedron PB = B->GetPolyhedron();
std::transform( PB.points_begin(), PB.points_end(), PB.points_begin(), t_rot_trans);
//calculate plane equations
std::transform( PA.facets_begin(), PA.facets_end(), PA.planes_begin(),Plane_equation());
std::transform( PB.facets_begin(), PB.facets_end(), PB.planes_begin(),Plane_equation());
//call test
return do_intersect(PA,PB);
}
//**********************************************************************************
//print polyhedron in actual position
void PrintPolyhedraActualPos(const shared_ptr<Shape>& cm1,const State& state1){
const Se3r& se3=state1.se3;
Polyhedra* A = static_cast<Polyhedra*>(cm1.get());
A->Initialize();
//move and rotate CGAL structure Polyhedron
Matrix3r rot_mat = (se3.orientation).toRotationMatrix();
Vector3r trans_vec = se3.position;
Transformation t_rot_trans(rot_mat(0,0),rot_mat(0,1),rot_mat(0,2), trans_vec[0],rot_mat(1,0),rot_mat(1,1),rot_mat(1,2),trans_vec[1],rot_mat(2,0),rot_mat(2,1),rot_mat(2,2),trans_vec[2],1.);
Polyhedron PA = A->GetPolyhedron();
std::transform( PA.points_begin(), PA.points_end(), PA.points_begin(), t_rot_trans);
PrintPolyhedron(PA);
}
/// Computes rotation matrix according to the z,x',z'' convention
inline MathLib::DenseMatrix<double, std::size_t>
getRotMat(double alpha, double beta, double gamma)
{
MathLib::DenseMatrix<double, std::size_t> rot_mat(3,3);
rot_mat(0,0) = cos(alpha)*cos(gamma) - sin(alpha)*cos(beta)*sin(gamma);
rot_mat(0,1) = sin(alpha)*cos(gamma) + cos(alpha)*cos(beta)*sin(gamma);
rot_mat(0,2) = sin(beta)*sin(gamma);
rot_mat(1,0) = -cos(alpha)*sin(gamma) - sin(alpha)*cos(beta)*cos(gamma);
rot_mat(1,1) = -sin(alpha)*sin(gamma) + cos(alpha)*cos(beta)*cos(gamma);
rot_mat(1,2) = sin(beta)*cos(gamma);
rot_mat(2,0) = sin(alpha)*sin(beta);
rot_mat(2,1) = -cos(alpha)*sin(beta);
rot_mat(2,2) = cos(beta);
return rot_mat;
}
// quaternion does not need to be normalized
void LLQuaternion::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const
{
LLMatrix3 rot_mat(*this);
rot_mat.orthogonalize();
rot_mat.getEulerAngles(roll, pitch, yaw);
// // NOTE: LLQuaternion's are actually inverted with respect to
// // the matrices, so this code also assumes inverted quaternions
// // (-x, -y, -z, w). The result is that roll,pitch,yaw are applied
// // in reverse order (yaw,pitch,roll).
// F32 x = -mQ[VX], y = -mQ[VY], z = -mQ[VZ], w = mQ[VW];
// F64 m20 = 2.0*(x*z-y*w);
// if (1.0f - fabsf(m20) < F_APPROXIMATELY_ZERO)
// {
// *roll = 0.0f;
// *pitch = (F32)asin(m20);
// *yaw = (F32)atan2(2.0*(x*y-z*w), 1.0 - 2.0*(x*x+z*z));
// }
// else
// {
// *roll = (F32)atan2(-2.0*(y*z+x*w), 1.0-2.0*(x*x+y*y));
// *pitch = (F32)asin(m20);
// *yaw = (F32)atan2(-2.0*(x*y+z*w), 1.0-2.0*(y*y+z*z));
// }
}
void PolygonFace::rotate(const glm::mat4 &rotation_matrix)
{
glm::mat3 rot_mat(rotation_matrix);
for (unsigned int i = 0; i < vertices_.size(); ++i) {
(*(vertices_[i])) = rot_mat * (*(vertices_[i]));
}
}
// Loads pattern images for detection
int LoadPattern(const char* filename, vector<Mat>& library, int& patternCount) {
Mat img = imread(filename, 0);
if (img.cols != img.rows){
return -1;
printf("Not a square pattern");
}
int msize = PAT_SIZE;
Mat src(msize, msize, CV_8UC1);
Point2f center((msize - 1) / 2.0f, (msize - 1) / 2.0f);
Mat rot_mat(2, 3, CV_32F);
resize(img, src, Size(msize, msize));
Mat subImg = src(Range(msize / 4, 3 * msize / 4), Range(msize / 4, 3 * msize / 4));
library.push_back(subImg);
rot_mat = getRotationMatrix2D(center, 90, 1.0);
for (int i = 1; i<4; i++){
Mat dst = Mat(msize, msize, CV_8UC1);
rot_mat = getRotationMatrix2D(center, -i * 90, 1.0);
warpAffine(src, dst, rot_mat, Size(msize, msize));
Mat subImg = dst(Range(msize / 4, 3 * msize / 4), Range(msize / 4, 3 * msize / 4));
library.push_back(subImg);
}
patternCount++;
return 1;
}
void TranslationRotation3D::updateR() {
Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_mat(
R_mat_);
Eigen::Map<Eigen::Vector3d> rot_axis_angle(R_);
Eigen::AngleAxis<double> tmp(rot_mat);
rot_axis_angle = tmp.angle() * tmp.axis();
}
void Camera::RotateV(double angle) {
double theta = DEG_TO_RAD(angle);
Matrix rotateVtheta = rot_mat(v, theta);
u = rotateVtheta * u;
w = rotateVtheta * w;
look = rotateVtheta * look;
}
void Camera::RotateU(double angle) {
double theta = DEG_TO_RAD(angle);
Matrix rotateUtheta = rot_mat(u, theta);
v = rotateUtheta * v;
w = rotateUtheta * w;
look = rotateUtheta * look;
}
void Camera::RotateW(double angle) {
double theta = DEG_TO_RAD(angle);
Matrix rotateWtheta = rot_mat(w, theta);
v = rotateWtheta * v;
u = rotateWtheta * u;
}
void rotate(t_env *e)
{
cl_float3 mat[3];
ident(e->data->mat);
rot_mat(mat, 0, e->rot.x);
mat_mult(e->data->mat, mat);
rot_mat(mat, 1, e->rot.y);
mat_mult(e->data->mat, mat);
rot_mat(mat, 2, e->rot.z);
mat_mult(e->data->mat, mat);
e->data->dir = v_mult_mat(e->data->mat, float3(0, 0, 1));
e->data->corners[0] = v_mult_mat(e->data->mat, e->corners[0]);
e->data->corners[1] = v_mult_mat(e->data->mat, e->corners[1]);
e->data->corners[2] = v_mult_mat(e->data->mat, e->corners[2]);
transpose(e->data->mat);
}
void Camera::RotateW(double angle) {
double a = angle*(PI/180);
Matrix r = rot_mat(w, a);
u = r * u;
v = r * v;
u.normalize();
v.normalize();
}
void Camera::RotateU(double angle) {
double a = angle*(PI/180);
Matrix r = rot_mat(u, a);
v = r * v;
w = r * w;
v.normalize();
w.normalize();
}
void Camera::RotateV(double angle) {
double a = angle*(PI/180);
Matrix r = rot_mat(v, a);
u = r * u;
w = r * w;
u.normalize();
w.normalize();
}
const LLQuaternion& LLQuaternion::setQuat(F32 roll, F32 pitch, F32 yaw)
{
LLMatrix3 rot_mat(roll, pitch, yaw);
rot_mat.orthogonalize();
*this = rot_mat.quaternion();
normalize();
return (*this);
}
void Camera::Rotate(Point p, Vector axis, double degrees) {
double a = degrees*(PI/180);
Matrix r = rot_mat(p, axis, a);
u = r * u;
v = r * v;
w = r * w;
u.normalize();
v.normalize();
w.normalize();
}
void
matrix_srt(struct matrix *mm, const struct srt *srt) {
if (!srt) {
return;
}
scale_mat(mm->m, srt->scalex, srt->scaley);
rot_mat(mm->m, srt->rot);
mm->m[4] += srt->offx;
mm->m[5] += srt->offy;
}
void Camera::Rotate(Point p, Vector axis, double degrees) {
double theta = DEG_TO_RAD(degrees);
Matrix rotationM = rot_mat(p, axis, theta);
u = rotationM * u;
v = rotationM * v;
w = rotationM * w;
look = rotationM * look;
}
void VRPN_CALLBACK VrpnTrackerRos::handle_accel(void *userData, const vrpn_TRACKERACCCB tracker_accel)
{
VrpnTrackerRos *tracker = static_cast<VrpnTrackerRos *>(userData);
ros::Publisher *accel_pub;
std::size_t sensor_index(0);
ros::NodeHandle nh = tracker->output_nh_;
if (tracker->process_sensor_id_)
{
sensor_index = static_cast<std::size_t>(tracker_accel.sensor);
nh = ros::NodeHandle(tracker->output_nh_, std::to_string(tracker_accel.sensor));
}
if (tracker->accel_pubs_.size() <= sensor_index)
{
tracker->accel_pubs_.resize(sensor_index + 1);
}
accel_pub = &(tracker->accel_pubs_[sensor_index]);
if (accel_pub->getTopic().empty())
{
*accel_pub = nh.advertise<geometry_msgs::TwistStamped>("accel", 1);
}
if (accel_pub->getNumSubscribers() > 0)
{
if (tracker->use_server_time_)
{
tracker->accel_msg_.header.stamp.sec = tracker_accel.msg_time.tv_sec;
tracker->accel_msg_.header.stamp.nsec = tracker_accel.msg_time.tv_usec * 1000;
}
else
{
tracker->accel_msg_.header.stamp = ros::Time::now();
}
tracker->accel_msg_.accel.linear.x = tracker_accel.acc[0];
tracker->accel_msg_.accel.linear.y = tracker_accel.acc[1];
tracker->accel_msg_.accel.linear.z = tracker_accel.acc[2];
double roll, pitch, yaw;
tf2::Matrix3x3 rot_mat(
tf2::Quaternion(tracker_accel.acc_quat[0], tracker_accel.acc_quat[1], tracker_accel.acc_quat[2],
tracker_accel.acc_quat[3]));
rot_mat.getRPY(roll, pitch, yaw);
tracker->accel_msg_.accel.angular.x = roll;
tracker->accel_msg_.accel.angular.y = pitch;
tracker->accel_msg_.accel.angular.z = yaw;
accel_pub->publish(tracker->accel_msg_);
}
}
//**********************************************************************************
//returns min coordinates
Vector3r MinCoord(const shared_ptr<Shape>& cm1,const State& state1){
const Se3r& se3=state1.se3;
Polyhedra* A = static_cast<Polyhedra*>(cm1.get());
//move and rotate CGAL structure Polyhedron
Matrix3r rot_mat = (se3.orientation).toRotationMatrix();
Vector3r trans_vec = se3.position;
Transformation t_rot_trans(rot_mat(0,0),rot_mat(0,1),rot_mat(0,2), trans_vec[0],rot_mat(1,0),rot_mat(1,1),rot_mat(1,2),trans_vec[1],rot_mat(2,0),rot_mat(2,1),rot_mat(2,2),trans_vec[2],1.);
Polyhedron PA = A->GetPolyhedron();
std::transform( PA.points_begin(), PA.points_end(), PA.points_begin(), t_rot_trans);
Vector3r minccord = trans_vec;
for(Polyhedron::Vertex_iterator vi = PA.vertices_begin(); vi != PA.vertices_end(); ++vi){
if (vi->point()[0]<minccord[0]) minccord[0]=vi->point()[0];
if (vi->point()[1]<minccord[1]) minccord[1]=vi->point()[1];
if (vi->point()[2]<minccord[2]) minccord[2]=vi->point()[2];
}
return minccord;
}
void TranslationRotation3D::updateR_mat() {
Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_mat(R_mat_);
Eigen::Map<const Eigen::Vector3d> rot_axis_angle(R_);
double angle = rot_axis_angle.norm();
if (angle < 1e-15) {
// identity matrix
rot_mat = Eigen::Matrix<double, 3, 3>::Identity();
} else {
rot_mat = Eigen::AngleAxis<double>(angle, rot_axis_angle / angle)
.toRotationMatrix();
}
}
inline void rotate(float angle, float rx, float ry, float rz)
{
float radians = (angle * float(M_PI)) / 180.0f;
float s = sinf(radians);
float c = cosf(radians);
float mag = sqrtf(rx * rx + ry * ry + rz * rz);
if(mag > 0.0f) {
rx /= mag;
ry /= mag;
rz /= mag;
mat4 rot_mat(
rx*rx*(1.0f-c)+c , ry*rx*(1.0f-c)-rz*s, rz*rx*(1.0f-c)+ry*s, 0.0f,
rx*ry*(1.0f-c)+rz*s, ry*ry*(1.0f-c)+c , rz*ry*(1.0f-c)-rx*s, 0.0f,
rx*rz*(1.0f-c)-ry*s, ry*rz*(1.0f-c)+rx*s, rz*rz*(1.0f-c)+c , 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
*this = rot_mat * *this;
}
}
size_t Pattern::loadPattern(const char* filename){
if( !fexists(filename) ) {
std::cerr<<"Unable to open "<<filename<<". Verify if the file exists and the rights are correctly set."<<std::endl;
return EXIT_FAILURE;
}
Mat img = imread(filename,0);
if(img.cols!=img.rows){
return -1;
std::cerr << "Not a square pattern" << std::endl;
}
int msize = patternSize;
Mat src(msize, msize, CV_8UC1);
Point2f center((msize-1)/2.0f,(msize-1)/2.0f);
Mat rot_mat(2,3,CV_32F);
resize(img, src, Size(msize,msize));
Mat subImg = src(Range(msize/4,3*msize/4), Range(msize/4,3*msize/4));
Pattern::patternLibrary.push_back(subImg);
rot_mat = getRotationMatrix2D( center, 90, 1.0);
for (int i=1; i<patternNbRotation; i++){
Mat dst= Mat(msize, msize, CV_8UC1);
rot_mat = getRotationMatrix2D( center, -i*90, 1.0);
warpAffine( src, dst , rot_mat, Size(msize,msize));
Mat subImg = dst(Range(msize/4,3*msize/4), Range(msize/4,3*msize/4));
Pattern::patternLibrary.push_back(subImg);
}
patternCount++;
return patternCount;
}
// -----------------------------------------------------------------------------
void LLViewerJoystick::moveFlycam(bool reset)
{
static LLQuaternion sFlycamRotation;
static LLVector3 sFlycamPosition;
static F32 sFlycamZoom;
if (!gFocusMgr.getAppHasFocus() || mDriverState != JDS_INITIALIZED
|| !gSavedSettings.getBOOL("JoystickEnabled") || !gSavedSettings.getBOOL("JoystickFlycamEnabled"))
{
return;
}
S32 axis[] =
{
gSavedSettings.getS32("JoystickAxis0"),
gSavedSettings.getS32("JoystickAxis1"),
gSavedSettings.getS32("JoystickAxis2"),
gSavedSettings.getS32("JoystickAxis3"),
gSavedSettings.getS32("JoystickAxis4"),
gSavedSettings.getS32("JoystickAxis5"),
gSavedSettings.getS32("JoystickAxis6")
};
bool in_build_mode = LLToolMgr::getInstance()->inBuildMode();
if (reset || mResetFlag)
{
sFlycamPosition = LLViewerCamera::getInstance()->getOrigin();
sFlycamRotation = LLViewerCamera::getInstance()->getQuaternion();
sFlycamZoom = LLViewerCamera::getInstance()->getView();
resetDeltas(axis);
return;
}
F32 axis_scale[] =
{
gSavedSettings.getF32("FlycamAxisScale0"),
gSavedSettings.getF32("FlycamAxisScale1"),
gSavedSettings.getF32("FlycamAxisScale2"),
gSavedSettings.getF32("FlycamAxisScale3"),
gSavedSettings.getF32("FlycamAxisScale4"),
gSavedSettings.getF32("FlycamAxisScale5"),
gSavedSettings.getF32("FlycamAxisScale6")
};
F32 dead_zone[] =
{
gSavedSettings.getF32("FlycamAxisDeadZone0"),
gSavedSettings.getF32("FlycamAxisDeadZone1"),
gSavedSettings.getF32("FlycamAxisDeadZone2"),
gSavedSettings.getF32("FlycamAxisDeadZone3"),
gSavedSettings.getF32("FlycamAxisDeadZone4"),
gSavedSettings.getF32("FlycamAxisDeadZone5"),
gSavedSettings.getF32("FlycamAxisDeadZone6")
};
F32 time = gFrameIntervalSeconds;
// avoid making ridiculously big movements if there's a big drop in fps
if (time > .2f)
{
time = .2f;
}
F32 cur_delta[7];
F32 feather = gSavedSettings.getF32("FlycamFeathering");
bool absolute = gSavedSettings.getBOOL("Cursor3D");
bool is_zero = true;
for (U32 i = 0; i < 7; i++)
{
cur_delta[i] = -getJoystickAxis(axis[i]);
F32 tmp = cur_delta[i];
if (absolute)
{
cur_delta[i] = cur_delta[i] - sLastDelta[i];
}
sLastDelta[i] = tmp;
if (cur_delta[i] > 0)
{
cur_delta[i] = llmax(cur_delta[i]-dead_zone[i], 0.f);
}
else
{
cur_delta[i] = llmin(cur_delta[i]+dead_zone[i], 0.f);
}
// we need smaller camera movements in build mode
// NOTE: this needs to remain after the deadzone calculation, otherwise
// we have issues with flycam "jumping" when the build dialog is opened/closed -Nyx
if (in_build_mode)
{
if (i == X_I || i == Y_I || i == Z_I)
{
cur_delta[i] /= BUILDMODE_FLYCAM_T_SCALE;
}
}
cur_delta[i] *= axis_scale[i];
if (!absolute)
{
cur_delta[i] *= time;
}
sDelta[i] = sDelta[i] + (cur_delta[i]-sDelta[i])*time*feather;
is_zero = is_zero && (cur_delta[i] == 0.f);
}
// Clear AFK state if moved beyond the deadzone
if (!is_zero && gAwayTimer.getElapsedTimeF32() > MIN_AFK_TIME)
{
gAgent.clearAFK();
}
sFlycamPosition += LLVector3(sDelta) * sFlycamRotation;
LLMatrix3 rot_mat(sDelta[3], sDelta[4], sDelta[5]);
sFlycamRotation = LLQuaternion(rot_mat)*sFlycamRotation;
if (gSavedSettings.getBOOL("AutoLeveling"))
{
LLMatrix3 level(sFlycamRotation);
LLVector3 x = LLVector3(level.mMatrix[0]);
LLVector3 y = LLVector3(level.mMatrix[1]);
LLVector3 z = LLVector3(level.mMatrix[2]);
y.mV[2] = 0.f;
y.normVec();
level.setRows(x,y,z);
level.orthogonalize();
LLQuaternion quat(level);
sFlycamRotation = nlerp(llmin(feather*time,1.f), sFlycamRotation, quat);
}
if (gSavedSettings.getBOOL("ZoomDirect"))
{
sFlycamZoom = sLastDelta[6]*axis_scale[6]+dead_zone[6];
}
else
{
sFlycamZoom += sDelta[6];
}
LLMatrix3 mat(sFlycamRotation);
LLViewerCamera::getInstance()->setView(sFlycamZoom);
LLViewerCamera::getInstance()->setOrigin(sFlycamPosition);
LLViewerCamera::getInstance()->mXAxis = LLVector3(mat.mMatrix[0]);
LLViewerCamera::getInstance()->mYAxis = LLVector3(mat.mMatrix[1]);
LLViewerCamera::getInstance()->mZAxis = LLVector3(mat.mMatrix[2]);
}
void LLDrawPoolTree::renderTree(BOOL selecting)
{
LLGLState normalize(GL_NORMALIZE, TRUE);
// Bind the texture for this tree.
gGL.getTexUnit(sDiffTex)->bind(mTexturep.get(), TRUE);
U32 indices_drawn = 0;
glMatrixMode(GL_MODELVIEW);
for (std::vector<LLFace*>::iterator iter = mDrawFace.begin();
iter != mDrawFace.end(); iter++)
{
LLFace *face = *iter;
LLDrawable *drawablep = face->getDrawable();
if (drawablep->isDead() || !face->getVertexBuffer())
{
continue;
}
face->getVertexBuffer()->setBuffer(LLDrawPoolTree::VERTEX_DATA_MASK);
U16* indicesp = (U16*) face->getVertexBuffer()->getIndicesPointer();
// Render each of the trees
LLVOTree *treep = (LLVOTree *)drawablep->getVObj().get();
LLColor4U color(255,255,255,255);
if (!selecting || treep->mGLName != 0)
{
if (selecting)
{
S32 name = treep->mGLName;
color = LLColor4U((U8)(name >> 16), (U8)(name >> 8), (U8)name, 255);
}
gGLLastMatrix = NULL;
glLoadMatrixd(gGLModelView);
//glPushMatrix();
F32 mat[16];
for (U32 i = 0; i < 16; i++)
mat[i] = (F32) gGLModelView[i];
LLMatrix4 matrix(mat);
// Translate to tree base HACK - adjustment in Z plants tree underground
const LLVector3 &pos_agent = treep->getPositionAgent();
//glTranslatef(pos_agent.mV[VX], pos_agent.mV[VY], pos_agent.mV[VZ] - 0.1f);
LLMatrix4 trans_mat;
trans_mat.setTranslation(pos_agent.mV[VX], pos_agent.mV[VY], pos_agent.mV[VZ] - 0.1f);
trans_mat *= matrix;
// Rotate to tree position and bend for current trunk/wind
// Note that trunk stiffness controls the amount of bend at the trunk as
// opposed to the crown of the tree
//
const F32 TRUNK_STIFF = 22.f;
LLQuaternion rot =
LLQuaternion(treep->mTrunkBend.magVec()*TRUNK_STIFF*DEG_TO_RAD, LLVector4(treep->mTrunkBend.mV[VX], treep->mTrunkBend.mV[VY], 0)) *
LLQuaternion(90.f*DEG_TO_RAD, LLVector4(0,0,1)) *
treep->getRotation();
LLMatrix4 rot_mat(rot);
rot_mat *= trans_mat;
F32 radius = treep->getScale().magVec()*0.05f;
LLMatrix4 scale_mat;
scale_mat.mMatrix[0][0] =
scale_mat.mMatrix[1][1] =
scale_mat.mMatrix[2][2] = radius;
scale_mat *= rot_mat;
const F32 THRESH_ANGLE_FOR_BILLBOARD = 15.f;
const F32 BLEND_RANGE_FOR_BILLBOARD = 3.f;
F32 droop = treep->mDroop + 25.f*(1.f - treep->mTrunkBend.magVec());
S32 stop_depth = 0;
F32 app_angle = treep->getAppAngle()*LLVOTree::sTreeFactor;
F32 alpha = 1.0;
S32 trunk_LOD = LLVOTree::sMAX_NUM_TREE_LOD_LEVELS;
for (S32 j = 0; j < 4; j++)
{
if (app_angle > LLVOTree::sLODAngles[j])
{
trunk_LOD = j;
break;
}
}
if(trunk_LOD >= LLVOTree::sMAX_NUM_TREE_LOD_LEVELS)
{
continue ; //do not render.
}
if (app_angle < (THRESH_ANGLE_FOR_BILLBOARD - BLEND_RANGE_FOR_BILLBOARD))
{
//
// Draw only the billboard
//
// Only the billboard, can use closer to normal alpha func.
stop_depth = -1;
LLFacePool::LLOverrideFaceColor clr(this, color);
indices_drawn += treep->drawBranchPipeline(scale_mat, indicesp, trunk_LOD, stop_depth, treep->mDepth, treep->mTrunkDepth, 1.0, treep->mTwist, droop, treep->mBranches, alpha);
}
else // if (app_angle > (THRESH_ANGLE_FOR_BILLBOARD + BLEND_RANGE_FOR_BILLBOARD))
{
//
// Draw only the full geometry tree
//
//stop_depth = (app_angle < THRESH_ANGLE_FOR_RECURSION_REDUCTION);
LLFacePool::LLOverrideFaceColor clr(this, color);
indices_drawn += treep->drawBranchPipeline(scale_mat, indicesp, trunk_LOD, stop_depth, treep->mDepth, treep->mTrunkDepth, 1.0, treep->mTwist, droop, treep->mBranches, alpha);
}
//glPopMatrix();
}
}
}
void imageCallback(
const sensor_msgs::ImageConstPtr& l_image_msg,
const sensor_msgs::ImageConstPtr& r_image_msg,
const sensor_msgs::CameraInfoConstPtr& l_info_msg,
const sensor_msgs::CameraInfoConstPtr& r_info_msg)
{
bool first_run = false;
// create odometer if not exists
if (!visual_odometer_)
{
first_run = true;
#if(DEBUG)
cv_bridge::CvImageConstPtr l_cv_ptr, r_cv_ptr;
l_cv_ptr = cv_bridge::toCvShare(l_image_msg, sensor_msgs::image_encodings::MONO8);
r_cv_ptr = cv_bridge::toCvShare(r_image_msg, sensor_msgs::image_encodings::MONO8);
last_l_image_ = l_cv_ptr->image;
last_r_image_ = r_cv_ptr->image;
#endif
initOdometer(l_info_msg, r_info_msg);
}
// convert images if necessary
uint8_t *l_image_data, *r_image_data;
int l_step, r_step;
cv_bridge::CvImageConstPtr l_cv_ptr, r_cv_ptr;
l_cv_ptr = cv_bridge::toCvShare(l_image_msg, sensor_msgs::image_encodings::MONO8);
l_image_data = l_cv_ptr->image.data;
l_step = l_cv_ptr->image.step[0];
r_cv_ptr = cv_bridge::toCvShare(r_image_msg, sensor_msgs::image_encodings::MONO8);
r_image_data = r_cv_ptr->image.data;
r_step = r_cv_ptr->image.step[0];
ROS_ASSERT(l_step == r_step);
ROS_ASSERT(l_image_msg->width == r_image_msg->width);
ROS_ASSERT(l_image_msg->height == r_image_msg->height);
int32_t dims[] = {l_image_msg->width, l_image_msg->height, l_step};
// on first run or when odometer got lost, only feed the odometer with
// images without retrieving data
if (first_run || got_lost_)
{
visual_odometer_->process(l_image_data, r_image_data, dims);
got_lost_ = false;
}
else
{
bool success = visual_odometer_->process(
l_image_data, r_image_data, dims, last_motion_small_);
if (success)
{
Matrix motion = Matrix::inv(visual_odometer_->getMotion());
ROS_DEBUG("Found %i matches with %i inliers.",
visual_odometer_->getNumberOfMatches(),
visual_odometer_->getNumberOfInliers());
ROS_DEBUG_STREAM("libviso2 returned the following motion:\n" << motion);
Matrix camera_motion;
// if image was replaced due to small motion we have to subtract the
// last motion to get the increment
if (last_motion_small_)
{
camera_motion = Matrix::inv(reference_motion_) * motion;
}
else
{
// image was not replaced, report full motion from odometer
camera_motion = motion;
}
reference_motion_ = motion; // store last motion as reference
std::cout<< camera_motion << "\n" <<std::endl;
#if (USE_MOVEMENT_CONSTRAIN)
double deltaRoll = atan2(camera_motion.val[2][1], camera_motion.val[2][2]);
double deltaPitch = asin(-camera_motion.val[2][0]);
double deltaYaw = atan2(camera_motion.val[1][0], camera_motion.val[0][0]);
double tanRoll = camera_motion.val[2][1] / camera_motion.val[2][2];
double tanPitch = tan(deltaPitch);
printf("deltaroll is %lf\n", deltaRoll);
printf("deltaPitch is %lf\n", deltaPitch);
printf("deltaYaw is %lf\n", deltaYaw);
double deltaX = camera_motion.val[0][3];
double deltaY = camera_motion.val[1][3];
double deltaZ = camera_motion.val[2][3];
printf("dx %lf, dy %lf, dz %lf, tanRoll %lf tanPitch %lf\n",deltaX, deltaY, deltaZ, tanRoll, tanPitch);
if(deltaY > 0 && deltaY > tanRoll * deltaZ)
{
camera_motion.val[1][3] = tanRoll * deltaZ;
//printf("dy %lf deltaY, dynew %lf\n", deltaY,camera_motion.val[2][3]);
}
else if(deltaY < 0 && deltaY < -tanRoll * deltaZ)
{
camera_motion.val[1][3] = -tanRoll * deltaZ;
//printf("dy %lf deltaY, dynew %lf\n", deltaY,camera_motion.val[2][3]);
}
/*if(deltaX > 0 && deltaX > tanPitch * deltaZ)
{
camera_motion.val[0][3] = tanPitch * deltaZ;
printf("dx %lf, dxnew %lf\n", deltaX,camera_motion.val[1][3]);
}
else if(deltaX < 0 && deltaX < -tanPitch * deltaZ)
{
camera_motion.val[0][3] = -tanPitch * deltaZ;
printf("dx %lf, dxnew %lf\n", deltaX,camera_motion.val[1][3]);
}*/
/*
if(deltaPitch > 0)
{
deltaPitch = deltaPitch+fabs(deltaRoll)+fabs(deltaYaw);
}
else
{
deltaPitch = deltaPitch-fabs(deltaRoll)-fabs(deltaYaw);
}*/
deltaPitch = deltaPitch+deltaYaw;
#endif
// calculate current feature flow
std::vector<Matcher::p_match> matches = visual_odometer_->getMatches();
std::vector<int> inlier_indices = visual_odometer_->getInlierIndices();
#if(DEBUG)
cv::Mat img1 = l_cv_ptr->image;
cv::Mat img2 = r_cv_ptr->image;
cv::Size size(last_l_image_.cols, last_l_image_.rows+last_r_image_.rows);
cv::Mat outImg(size,CV_MAKETYPE(img1.depth(), 3));
cv::Mat outImg1 = outImg( cv::Rect(0, 0, last_l_image_.cols, last_l_image_.rows) );
cv::Mat outImg2 = outImg( cv::Rect(0, last_l_image_.rows, img1.cols, img1.rows) );
if( last_l_image_.type() == CV_8U )
cvtColor( last_l_image_, outImg1, CV_GRAY2BGR );
else
last_l_image_.copyTo( outImg1 );
if( img1.type() == CV_8U )
cvtColor( img1, outImg2, CV_GRAY2BGR );
else
img1.copyTo( outImg2 );
for (size_t i = 0; i < matches.size(); ++i)
{
cv::Point pt1(matches[i].u1p,matches[i].v1p);
cv::Point pt2(matches[i].u1c,matches[i].v1c + last_l_image_.rows);
if(pt1.y > 239)
cv::line(outImg, pt1, pt2, cv::Scalar(255,0,0));
//else
//cv::line(outImg, pt1, pt2, cv::Scalar(0,255,0));
}
cv::imshow("matching image", outImg);
cv::waitKey(10);
last_l_image_ = img1;
last_r_image_ = img2;
#endif
double feature_flow = computeFeatureFlow(matches);
last_motion_small_ = (feature_flow < motion_threshold_);
ROS_DEBUG_STREAM("Feature flow is " << feature_flow
<< ", marking last motion as "
<< (last_motion_small_ ? "small." : "normal."));
btMatrix3x3 rot_mat(
cos(deltaPitch), 0, sin(deltaPitch),
0, 1, 0,
-sin(deltaPitch), 0, cos(deltaPitch));
/*btMatrix3x3 rot_mat(
camera_motion.val[0][0], camera_motion.val[0][1], camera_motion.val[0][2],
camera_motion.val[1][0], camera_motion.val[1][1], camera_motion.val[1][2],
camera_motion.val[2][0], camera_motion.val[2][1], camera_motion.val[2][2]);*/
btVector3 t(camera_motion.val[0][3], camera_motion.val[1][3], camera_motion.val[2][3]);
//rotation
/*double delta_yaw = 0;
double delta_pitch = 0;
double delta_roll = 0;
delta_yaw = delta_yaw*M_PI/180.0;
delta_pitch = delta_pitch*M_PI/180.0;
delta_roll = delta_roll*M_PI/180.0;
//btMatrix3x3 initialTrans;
Eigen::Matrix4f initialTrans = Eigen::Matrix4f::Identity();
initialTrans(0,0) = cos(delta_pitch)*cos(delta_yaw);
initialTrans(0,1) = -cos(delta_roll)*sin(delta_yaw) + sin(delta_roll)*sin(delta_pitch)*cos(delta_yaw);
initialTrans(0,2) = sin(delta_roll)*sin(delta_yaw) + cos(delta_roll)*sin(delta_pitch)*cos(delta_yaw);
initialTrans(1,0) = cos(delta_pitch)*sin(delta_yaw);
initialTrans(1,1) = cos(delta_roll)*cos(delta_yaw) + sin(delta_roll)*sin(delta_pitch)*sin(delta_yaw);
initialTrans(1,2) = -sin(delta_roll)*cos(delta_yaw) + cos(delta_roll)*sin(delta_pitch)*sin(delta_yaw);
initialTrans(2,0) = -sin(delta_pitch);
initialTrans(2,1) = sin(delta_roll)*cos(delta_pitch);
initialTrans(2,2) = cos(delta_roll)*cos(delta_pitch);
btMatrix3x3 rot_mat(
initialTrans(0,0), initialTrans(0,1), initialTrans(0,2),
initialTrans(1,0), initialTrans(1,1), initialTrans(1,2),
initialTrans(2,0), initialTrans(2,1), initialTrans(2,2));
btVector3 t(0.0, 0.00, 0.01);*/
tf::Transform delta_transform(rot_mat, t);
setPoseCovariance(STANDARD_POSE_COVARIANCE);
setTwistCovariance(STANDARD_TWIST_COVARIANCE);
integrateAndPublish(delta_transform, l_image_msg->header.stamp);
if (point_cloud_pub_.getNumSubscribers() > 0)
{
computeAndPublishPointCloud(l_info_msg, l_image_msg, r_info_msg, matches, inlier_indices);
}
}
else
{
setPoseCovariance(BAD_COVARIANCE);
setTwistCovariance(BAD_COVARIANCE);
tf::Transform delta_transform;
delta_transform.setIdentity();
integrateAndPublish(delta_transform, l_image_msg->header.stamp);
ROS_DEBUG("Call to VisualOdometryStereo::process() failed.");
ROS_WARN_THROTTLE(1.0, "Visual Odometer got lost!");
got_lost_ = true;
}
}
}
void
CrackFrontDefinition::updateCrackFrontGeometry()
{
updateDataForCrackDirection();
_segment_lengths.clear();
_tangent_directions.clear();
_crack_directions.clear();
_rot_matrix.clear();
if (_treat_as_2d)
{
RealVectorValue tangent_direction;
RealVectorValue crack_direction;
tangent_direction(_axis_2d) = 1.0;
_tangent_directions.push_back(tangent_direction);
const Node* crack_front_node = _mesh.nodePtr(_ordered_crack_front_nodes[0]);
crack_direction = calculateCrackFrontDirection(crack_front_node,tangent_direction,MIDDLE_NODE);
_crack_directions.push_back(crack_direction);
_crack_plane_normal = crack_direction.cross(tangent_direction);
ColumnMajorMatrix rot_mat;
rot_mat(0,0) = crack_direction(0);
rot_mat(0,1) = crack_direction(1);
rot_mat(0,2) = crack_direction(2);
rot_mat(1,0) = _crack_plane_normal(0);
rot_mat(1,1) = _crack_plane_normal(1);
rot_mat(1,2) = _crack_plane_normal(2);
rot_mat(2,0) = 0.0;
rot_mat(2,1) = 0.0;
rot_mat(2,2) = 0.0;
rot_mat(2,_axis_2d) = 1.0;
_rot_matrix.push_back(rot_mat);
_segment_lengths.push_back(std::make_pair(0.0,0.0));
_overall_length = 0.0;
}
else
{
unsigned int num_crack_front_nodes = _ordered_crack_front_nodes.size();
std::vector<Node*> crack_front_nodes;
crack_front_nodes.reserve(num_crack_front_nodes);
_segment_lengths.reserve(num_crack_front_nodes);
_tangent_directions.reserve(num_crack_front_nodes);
_crack_directions.reserve(num_crack_front_nodes);
for (unsigned int i=0; i<num_crack_front_nodes; ++i)
{
crack_front_nodes.push_back(_mesh.nodePtr(_ordered_crack_front_nodes[i]));
}
_overall_length = 0.0;
RealVectorValue back_segment;
Real back_segment_len = 0.0;
for (unsigned int i=0; i<num_crack_front_nodes; ++i)
{
CRACK_NODE_TYPE ntype = MIDDLE_NODE;
if (i==0)
{
ntype = END_1_NODE;
}
else if (i==num_crack_front_nodes-1)
{
ntype = END_2_NODE;
}
RealVectorValue forward_segment;
Real forward_segment_len = 0.0;
if (ntype != END_2_NODE)
{
forward_segment = *crack_front_nodes[i+1] - *crack_front_nodes[i];
forward_segment_len = forward_segment.size();
}
_segment_lengths.push_back(std::make_pair(back_segment_len,forward_segment_len));
//Moose::out<<"seg len: "<<back_segment_len<<" "<<forward_segment_len<<std::endl;
RealVectorValue tangent_direction = back_segment + forward_segment;
tangent_direction = tangent_direction / tangent_direction.size();
_tangent_directions.push_back(tangent_direction);
//Moose::out<<"tan dir: "<<tangent_direction(0)<<" "<<tangent_direction(1)<<" "<<tangent_direction(2)<<std::endl;
_crack_directions.push_back(calculateCrackFrontDirection(crack_front_nodes[i],tangent_direction,ntype));
_overall_length += forward_segment_len;
back_segment = forward_segment;
back_segment_len = forward_segment_len;
}
//For CURVED_CRACK_FRONT, _crack_plane_normal gets computed in updateDataForCrackDirection
if (_direction_method != CURVED_CRACK_FRONT)
{
unsigned int mid_id = (num_crack_front_nodes-1)/2;
_crack_plane_normal = _tangent_directions[mid_id].cross(_crack_directions[mid_id]);
//Make sure the normal vector is non-zero
RealVectorValue zero_vec(0.0);
if (_crack_plane_normal.absolute_fuzzy_equals(zero_vec,1.e-15))
mooseError("Crack plane normal vector evaluates to zero");
}
// Create rotation matrix
for (unsigned int i=0; i<num_crack_front_nodes; ++i)
{
ColumnMajorMatrix rot_mat;
rot_mat(0,0) = _crack_directions[i](0);
rot_mat(0,1) = _crack_directions[i](1);
rot_mat(0,2) = _crack_directions[i](2);
rot_mat(1,0) = _crack_plane_normal(0);
rot_mat(1,1) = _crack_plane_normal(1);
rot_mat(1,2) = _crack_plane_normal(2);
rot_mat(2,0) = _tangent_directions[i](0);
rot_mat(2,1) = _tangent_directions[i](1);
rot_mat(2,2) = _tangent_directions[i](2);
_rot_matrix.push_back(rot_mat);
}
Moose::out<<"Summary of J-Integral crack front geometry:"<<std::endl;
Moose::out<<"index node id x coord y coord z coord x dir y dir z dir seg length"<<std::endl;
for (unsigned int i=0; i<crack_front_nodes.size(); ++i)
{
Moose::out<<std::left
<<std::setw(8) <<i+1
<<std::setw(10)<<crack_front_nodes[i]->id()
<<std::setw(14)<<(*crack_front_nodes[i])(0)
<<std::setw(14)<<(*crack_front_nodes[i])(1)
<<std::setw(14)<<(*crack_front_nodes[i])(2)
<<std::setw(14)<<_crack_directions[i](0)
<<std::setw(14)<<_crack_directions[i](1)
<<std::setw(14)<<_crack_directions[i](2)
<<std::setw(14)<<(_segment_lengths[i].first+_segment_lengths[i].second)/2.0
<<std::endl;
}
Moose::out<<"overall length: "<<_overall_length<<std::endl;
}
}
/**
* MEX function entry point.
*/
void
mexFunction(
int nlhs,
mxArray *plhs[],
int nrhs,
const mxArray *prhs[]
) {
double *q, *qd, *qdd;
double *tau;
unsigned int m,n;
int j, njoints, p, nq;
double *fext = NULL;
double *grav = NULL;
Robot robot;
mxArray *link0;
mxArray *mx_robot;
mxArray *mx_links;
static int first_time = 0;
/*
fprintf(stderr, "Fast RNE: (c) Peter Corke 2002-2011\n");
*/
if ( !mxIsClass(ROBOT_IN, "SerialLink") )
mexErrMsgTxt("frne: first argument is not a robot structure\n");
mx_robot = (mxArray *)ROBOT_IN;
njoints = mstruct_getint(mx_robot, 0, "n");
/***********************************************************************
* Handle the different calling formats.
* Setup pointers to q, qd and qdd inputs
***********************************************************************/
switch (nrhs) {
case 2:
/*
* TAU = RNE(ROBOT, [Q QD QDD])
*/
if (NUMCOLS(A1_IN) != 3 * njoints)
mexErrMsgTxt("frne: too few cols in [Q QD QDD]");
q = POINTER(A1_IN);
nq = NUMROWS(A1_IN);
qd = &q[njoints*nq];
qdd = &q[2*njoints*nq];
break;
case 3:
/*
* TAU = RNE(ROBOT, [Q QD QDD], GRAV)
*/
if (NUMCOLS(A1_IN) != (3 * njoints))
mexErrMsgTxt("frne: too few cols in [Q QD QDD]");
q = POINTER(A1_IN);
nq = NUMROWS(A1_IN);
qd = &q[njoints*nq];
qdd = &q[2*njoints*nq];
if (NUMELS(A2_IN) != 3)
mexErrMsgTxt("frne: gravity vector expected");
grav = POINTER(A2_IN);
break;
case 4:
/*
* TAU = RNE(ROBOT, Q, QD, QDD)
* TAU = RNE(ROBOT, [Q QD QDD], GRAV, FEXT)
*/
if (NUMCOLS(A1_IN) == (3 * njoints)) {
q = POINTER(A1_IN);
nq = NUMROWS(A1_IN);
qd = &q[njoints*nq];
qdd = &q[2*njoints*nq];
if (NUMELS(A2_IN) != 3)
mexErrMsgTxt("frne: gravity vector expected");
grav = POINTER(A2_IN);
if (NUMELS(A3_IN) != 6)
mexErrMsgTxt("frne: Fext vector expected");
fext = POINTER(A3_IN);
} else {
int nqd = NUMROWS(A2_IN),
nqdd = NUMROWS(A3_IN);
nq = NUMROWS(A1_IN);
if ((nq != nqd) || (nqd != nqdd))
mexErrMsgTxt("frne: Q QD QDD must be same length");
if ( (NUMCOLS(A1_IN) != njoints) ||
(NUMCOLS(A2_IN) != njoints) ||
(NUMCOLS(A3_IN) != njoints)
)
mexErrMsgTxt("frne: Q must have Naxis columns");
q = POINTER(A1_IN);
qd = POINTER(A2_IN);
qdd = POINTER(A3_IN);
}
break;
case 5: {
/*
* TAU = RNE(ROBOT, Q, QD, QDD, GRAV)
*/
int nqd = NUMROWS(A2_IN),
nqdd = NUMROWS(A3_IN);
nq = NUMROWS(A1_IN);
if ((nq != nqd) || (nqd != nqdd))
mexErrMsgTxt("frne: Q QD QDD must be same length");
if ( (NUMCOLS(A1_IN) != njoints) ||
(NUMCOLS(A2_IN) != njoints) ||
(NUMCOLS(A3_IN) != njoints)
)
mexErrMsgTxt("frne: Q must have Naxis columns");
q = POINTER(A1_IN);
qd = POINTER(A2_IN);
qdd = POINTER(A3_IN);
if (NUMELS(A4_IN) != 3)
mexErrMsgTxt("frne: gravity vector expected");
grav = POINTER(A4_IN);
break;
}
case 6: {
/*
* TAU = RNE(ROBOT, Q, QD, QDD, GRAV, FEXT)
*/
int nqd = NUMROWS(A2_IN),
nqdd = NUMROWS(A3_IN);
nq = NUMROWS(A1_IN);
if ((nq != nqd) || (nqd != nqdd))
mexErrMsgTxt("frne: Q QD QDD must be same length");
if ( (NUMCOLS(A1_IN) != njoints) ||
(NUMCOLS(A2_IN) != njoints) ||
(NUMCOLS(A3_IN) != njoints)
)
mexErrMsgTxt("frne: Q must have Naxis columns");
q = POINTER(A1_IN);
qd = POINTER(A2_IN);
qdd = POINTER(A3_IN);
if (NUMELS(A4_IN) != 3)
mexErrMsgTxt("frne: gravity vector expected");
grav = POINTER(A4_IN);
if (NUMELS(A5_IN) != 6)
mexErrMsgTxt("frne: Fext vector expected");
fext = POINTER(A5_IN);
break;
}
default:
mexErrMsgTxt("frne: wrong number of arguments.");
}
/*
* fill out the robot structure
*/
robot.njoints = njoints;
if (grav)
robot.gravity = (Vect *)grav;
else
robot.gravity = (Vect *)mxGetPr( mxGetProperty(mx_robot, (mwIndex)0, "gravity") );
robot.dhtype = mstruct_getint(mx_robot, 0, "mdh");
/* build link structure */
robot.links = (Link *)mxCalloc((mwSize) njoints, (mwSize) sizeof(Link));
/***********************************************************************
* Now we have to get pointers to data spread all across a cell-array
* of Matlab structures.
*
* Matlab structure elements can be found by name (slow) or by number (fast).
* We assume that since the link structures are all created by the same
* constructor, the index number for each element will be the same for all
* links. However we make no assumption about the numbers themselves.
***********************************************************************/
/* get pointer to the first link structure */
link0 = mxGetProperty(mx_robot, (mwIndex) 0, "links");
if (link0 == NULL)
mexErrMsgTxt("couldnt find element link in robot object");
/*
* Elements of the link structure are:
*
* alpha:
* A:
* theta:
* D:
* offset:
* sigma:
* mdh:
* m:
* r:
* I:
* Jm:
* G:
* B:
* Tc:
*/
if (first_time == 0) {
mexPrintf("Fast RNE: (c) Peter Corke 2002-2012\n");
first_time = 1;
}
/*
* copy data from the Link objects into the local links structure
* to save function calls later
*/
for (j=0; j<njoints; j++) {
Link *l = &robot.links[j];
mxArray *links = mxGetProperty(mx_robot, (mwIndex) 0, "links"); // links array
l->alpha = mxGetScalar( mxGetProperty(links, (mwIndex) j, "alpha") );
l->A = mxGetScalar( mxGetProperty(links, (mwIndex) j, "a") );
l->theta = mxGetScalar( mxGetProperty(links, (mwIndex) j, "theta") );
l->D = mxGetScalar( mxGetProperty(links, (mwIndex) j, "d") );
l->sigma = mxGetScalar( mxGetProperty(links, (mwIndex) j, "sigma") );
l->offset = mxGetScalar( mxGetProperty(links, (mwIndex) j, "offset") );
l->m = mxGetScalar( mxGetProperty(links, (mwIndex) j, "m") );
l->rbar = (Vect *)mxGetPr( mxGetProperty(links, (mwIndex) j, "r") );
l->I = mxGetPr( mxGetProperty(links, (mwIndex) j, "I") );
l->Jm = mxGetScalar( mxGetProperty(links, (mwIndex) j, "Jm") );
l->G = mxGetScalar( mxGetProperty(links, (mwIndex) j, "G") );
l->B = mxGetScalar( mxGetProperty(links, (mwIndex) j, "B") );
l->Tc = mxGetPr( mxGetProperty(links, (mwIndex) j, "Tc") );
}
/* Create a matrix for the return argument */
TAU_OUT = mxCreateDoubleMatrix((mwSize) nq, (mwSize) njoints, mxREAL);
tau = mxGetPr(TAU_OUT);
#define MEL(x,R,C) (x[(R)+(C)*nq])
/* for each point in the input trajectory */
for (p=0; p<nq; p++) {
/*
* update all position dependent variables
*/
for (j = 0; j < njoints; j++) {
Link *l = &robot.links[j];
switch (l->sigma) {
case REVOLUTE:
rot_mat(l, MEL(q,p,j)+l->offset, l->D, robot.dhtype);
break;
case PRISMATIC:
rot_mat(l, l->theta, MEL(q,p,j)+l->offset, robot.dhtype);
break;
}
#ifdef DEBUG
rot_print("R", &l->R);
vect_print("p*", &l->r);
#endif
}
newton_euler(&robot, &tau[p], &qd[p], &qdd[p], fext, nq);
}
mxFree(robot.links);
}
TEST(GeoLib, SurfaceIsPointInSurface)
{
std::vector<std::function<double(double, double)>> surface_functions;
surface_functions.push_back(constant);
surface_functions.push_back(coscos);
for (auto f : surface_functions) {
std::random_device rd;
std::string name("Surface");
// generate ll and ur in random way
std::mt19937 random_engine_mt19937(rd());
std::normal_distribution<> normal_dist_ll(-10, 2);
std::normal_distribution<> normal_dist_ur(10, 2);
MathLib::Point3d ll(std::array<double,3>({{
normal_dist_ll(random_engine_mt19937),
normal_dist_ll(random_engine_mt19937),
0.0
}
}));
MathLib::Point3d ur(std::array<double,3>({{
normal_dist_ur(random_engine_mt19937),
normal_dist_ur(random_engine_mt19937),
0.0
}
}));
for (std::size_t k(0); k<3; ++k)
if (ll[k] > ur[k])
std::swap(ll[k], ur[k]);
// random discretization of the domain
std::default_random_engine re(rd());
std::uniform_int_distribution<std::size_t> uniform_dist(2, 25);
std::array<std::size_t,2> n_steps = {{uniform_dist(re),uniform_dist(re)}};
std::unique_ptr<MeshLib::Mesh> sfc_mesh(
MeshLib::MeshGenerator::createSurfaceMesh(
name, ll, ur, n_steps, f
)
);
// random rotation angles
std::normal_distribution<> normal_dist_angles(
0, boost::math::double_constants::two_pi);
std::array<double,3> euler_angles = {{
normal_dist_angles(random_engine_mt19937),
normal_dist_angles(random_engine_mt19937),
normal_dist_angles(random_engine_mt19937)
}
};
MathLib::DenseMatrix<double, std::size_t> rot_mat(getRotMat(
euler_angles[0], euler_angles[1], euler_angles[2]));
std::vector<MeshLib::Node*> const& nodes(sfc_mesh->getNodes());
GeoLib::rotatePoints<MeshLib::Node>(rot_mat, nodes);
MathLib::Vector3 const normal(0,0,1.0);
MathLib::Vector3 const surface_normal(rot_mat * normal);
double const eps(1e-6);
MathLib::Vector3 const displacement(eps * surface_normal);
GeoLib::GEOObjects geometries;
MeshLib::convertMeshToGeo(*sfc_mesh, geometries);
std::vector<GeoLib::Surface*> const& sfcs(*geometries.getSurfaceVec(name));
GeoLib::Surface const*const sfc(sfcs.front());
std::vector<GeoLib::Point*> const& pnts(*geometries.getPointVec(name));
// test triangle edge point of the surface triangles
for (auto const p : pnts) {
EXPECT_TRUE(sfc->isPntInSfc(*p));
MathLib::Point3d q(*p);
for (std::size_t k(0); k<3; ++k)
q[k] += displacement[k];
EXPECT_FALSE(sfc->isPntInSfc(q));
}
// test edge middle points of the triangles
for (std::size_t k(0); k<sfc->getNTriangles(); ++k) {
MathLib::Point3d p, q, r;
std::tie(p,q,r) = getEdgeMiddlePoints(*(*sfc)[k]);
EXPECT_TRUE(sfc->isPntInSfc(p));
EXPECT_TRUE(sfc->isPntInSfc(q));
EXPECT_TRUE(sfc->isPntInSfc(r));
}
}
}
void Num_Extract::extract_Number(Mat pre , vector<Mat>src ){
Mat rot_pre;
Scalar color = Scalar(255,255,255);
pre.copyTo(rot_pre);
vector<Mat>masked;
for(int i = 0 ; i<src.size() ; i++){
masked.push_back(src[i]);
}
/*for(int i = 0 ; i < masked.size() ; i++){
imshow("masked",masked[i]);
waitKey(0);
}*/
Mat grey,grey0,grey1;
//vector<Mat> bgr_planes;
vector<Vec4i> hierarchy;
std::vector<std::vector<cv::Point> > contour,ext_contour;
RotatedRect outrect;
Mat rot_mat( 2, 3, CV_32FC1 );
int out_ind;
vector<Rect> valid,valid1,boxes;//valid and valid1 are bounding rectangles after testing validity conditions
//boxes contains all bounding boxes
vector<int> valid_index,valid_index1;
for(int i = 0 ; i<masked.size() ; i++){
//split(masked[i],bgr_planes);
cvtColor(masked[i],grey1,CV_BGR2GRAY);
Canny(grey1,grey,0,256,5);
/*Canny(bgr_planes[0],grey1,0,256,5);
Canny(bgr_planes[1],grey2,0,256,5);
Canny(bgr_planes[2],grey3,0,256,5);
max(grey1,grey2,grey1);
max(grey1,grey3,grey);
max(grey,grey5,grey);//getting strongest edges*/
dilate(grey , grey0 , Mat() , Point(-1,-1));
grey = grey0;
cv::findContours(grey, ext_contour,CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
double areamax = 0;
int index;
for(int j = 0 ; j< ext_contour.size() ; j++){
if(contourArea(ext_contour[j],false)>areamax){
index = j;
areamax = contourArea(ext_contour[j],false);
}
}
outrect = minAreaRect(Mat(ext_contour[index]));//outer rectangle of the bin
float angle,width;
Point2f pts[4];
outrect.points(pts);
float dist1 = (sqrt((pts[0].y-pts[1].y)*(pts[0].y-pts[1].y) + (pts[0].x-pts[1].x)*(pts[0].x-pts[1].x)));
float dist2 = (sqrt((pts[0].y-pts[3].y)*(pts[0].y-pts[3].y) + (pts[0].x-pts[3].x)*(pts[0].x-pts[3].x)));
if (dist1>dist2) width = dist1;//getting the longer edge length of outrect
else width = dist2;
for(int j = 0 ; j<4 ; j++){
float dist = sqrt((pts[j].y-pts[(j+1)%4].y)*(pts[j].y-pts[(j+1)%4].y) + (pts[j].x-pts[(j+1)%4].x)*(pts[j].x-pts[(j+1)%4].x));
if(dist==width){
angle = atan((pts[j].y-pts[(j+1)%4].y)/(pts[(j+1)%4].x-pts[j].x));
}
}
Mat outrect_img = Mat::zeros(pre.size(),CV_8UC3);
/*for (int j = 0; j < 4; j++)
line(image, pts[j], pts[(j+1)%4], Scalar(0,255,0));
imshow("outrect" , outrect_img);
waitKey(0);*/
angle = angle * 180/3.14;
cout << angle <<endl;
if(angle<0){//building rotation matrices
rot_mat = getRotationMatrix2D(outrect.center,(-90-angle),1.0);
}
else{
rot_mat = getRotationMatrix2D(outrect.center,(90-angle),1.0);
}
warpAffine(grey1,grey0,rot_mat,grey0.size());//rotating to make the outer bin straight
//grey1 is the grayscale image (unrotated)
//after rotation stored in grey0
warpAffine(pre,rot_pre,rot_mat,rot_pre.size());//rotating the original (color) image by the same angle
Canny(grey0,grey,0,256,5);//thresholding the rotated image (grey0)
cv::findContours(grey, contour,hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
for(int j = 0 ; j<contour.size() ; j++){
boxes.push_back(boundingRect(Mat(contour[j])));
}//making boxes out of all contours
areamax = 0;
for(int j = 0 ; j<boxes.size() ; j++){
if(boxes[j].width*boxes[j].height > areamax){
areamax = boxes[j].width*boxes[j].height;
}
}//finding the box with the largest area
/*Mat all_contours = Mat::zeros(pre.size(),CV_8UC3);
for(int k = 0 ; k < contour.size() ; k++){
drawContours( all_contours, contour , k ,color , 1 ,8 ,vector<Vec4i>() ,0 , Point() );
}
imshow("all contours",all_contours);
waitKey(0);
*/
Mat box_n_contours = Mat::zeros(pre.size(),CV_8UC3);
for(int k = 0 ; k < contour.size() ; k++){
drawContours(box_n_contours , contour , k ,color , 1 ,8 ,vector<Vec4i>() ,0 , Point() );
if(boxes[k].width*boxes[k].height==areamax){
continue;
}
rectangle(box_n_contours , boxes[k] , color );
}
imshow("contours with boxes except outermost",box_n_contours);
waitKey(0);
for (int j = 0 ; j < boxes.size() ; j++){
if(boxes[j].width*boxes[j].height < 0.7*areamax && boxes[j].width*boxes[j].height > 0.05*areamax){
valid.push_back(boxes[j]);//Filtering boxes on the basis of their area (rejecting the small ones)
valid_index.push_back(j); //this is the first validating condition
}
}
for(int j = 0 ; j<valid.size() ; j++){
double aspect = valid[j].width/valid[j].height;
if(aspect < 1){//removing others on the basis of aspect ratio , second validating condition
valid1.push_back(valid[j]);//forming the list of valid bounding boxes
valid_index1.push_back(valid_index[j]);
}
}
Mat first_test_boxes = Mat::zeros(pre.size(),CV_8UC3);
for(int k = 0 ; k < valid.size() ; k++){
rectangle(first_test_boxes , valid[k] , color );
}
imshow("after first test ",first_test_boxes);
waitKey(0);
Mat final_boxes = Mat::zeros(pre.size(),CV_8UC3);
for(int k = 0 ; k < valid1.size() ; k++){
rectangle(final_boxes , valid1[k] , color );
drawContours(final_boxes , contour , valid_index1[k] ,color , 1 ,8 ,vector<Vec4i>() ,0 , Point() );
}//valid_index1 is required to draw the corresponding contours
imshow("final valid boxes and contours",final_boxes);
waitKey(0);
Rect box = valid1[0];
for(int j = 1 ; j<valid1.size() ; j++){ // now joining all valid boxes to extract the number
box = box | valid1[j];
}
Mat final_mask = Mat::zeros(pre.size(),CV_8UC3);
rectangle(final_mask , box , color ,CV_FILLED );//building the final mask
Mat ext_number = rot_pre & final_mask;//applying final_mask onto rot_pre
imshow("extracted no." , ext_number);
waitKey(0);
/*for(int j = 0 ; j<contour.size() ; j++){
if(hierarchy[j][3]!=-1){
valid.push_back(boundingRect(Mat(contour[j])));
}
}
for(int j = 0 ; j<valid.size() ; j++){
double aspect = valid[j].width/valid[j].height;
if(aspect < 1.5){//removing others on the basis of aspect ratio
valid1.push_back(valid[j]);//forming the list of valid bounding boxes
}
}
Rect box = valid1[0];
for(int j = 1 ; j<valid1.size() ; j++){
box = box | valid1[j];
}
Mat box_mat = Mat::zeros(rot_pre.size(),CV_8UC3);
Mat drawing = Mat::zeros(rot_pre.size(),CV_8UC3);
rectangle( box_mat, box , color , CV_FILLED );//drawing the rectangle on box_mat
rot_pre.copyTo(drawing,box_mat);//applying mask (box_mat) onto rot_pre and saving on drawing*/
dst.push_back(ext_number);//building output list
boxes.clear();
valid.clear();
valid1.clear();
valid_index.clear();
valid_index1.clear();
}
//cout<<dst.size()<<endl;
//cout<<valid.size()<<endl;
//cout<<valid1.size()<<endl;
}
