char BindTriangle::set(const XY* corner, const XY& at, const XYZ* positions, const XYZ& topos, const float* distance, triangle_bind_info& res)
{
//res.wei[0] = 1.0f;
//res.wei[1] = res.wei[2] = 0.0f;
XYZ dto = topos - positions[0];
if(dto.length() > distance[0]*2) return 0;
dto = topos - positions[1];
if(dto.length() > distance[1]*2) return 0;
dto = topos - positions[2];
if(dto.length() > distance[2]*2) return 0;
float f120 = barycentric_coord(corner[1].x, corner[1].y, corner[2].x, corner[2].y, corner[0].x, corner[0].y);
float f201 = barycentric_coord(corner[2].x, corner[2].y, corner[0].x, corner[0].y, corner[1].x, corner[1].y);
float f012 = barycentric_coord(corner[0].x, corner[0].y, corner[1].x, corner[1].y, corner[2].x, corner[2].y);
float alpha, beta, gamma;
alpha = barycentric_coord(corner[1].x,corner[1].y, corner[2].x, corner[2].y, at.x, at.y)/f120;
if(alpha<0 || alpha>1) return 0;
beta = barycentric_coord(corner[2].x, corner[2].y, corner[0].x, corner[0].y, at.x, at.y)/f201;
if(beta<0 || beta>1) return 0;
gamma = barycentric_coord(corner[0].x, corner[0].y, corner[1].x, corner[1].y, at.x, at.y)/f012;
if(gamma<0 || gamma>1) return 0;
res.wei[0] = alpha;
res.wei[1] = beta;
res.wei[2] = gamma;
return 1;
}
void TrialMol::SetBasis(const uint p1, const uint p2)
{
using namespace geom;
//W is unit vec of p1->p2
XYZ wVec = axes->MinImage(tCoords.Difference(p2, p1), box);
wVec.Normalize();
XYZ uVec;
//check to make sure our W isn't in line with the standard X Axis
if (wVec.x < 0.8) {
//V will be W x the standard X unit vec
uVec = XYZ(1.0, 0.0, 0.0);
}
else {
//V will be W x the standard Y unit vec
uVec = XYZ(0.0, 1.0, 0.0);
}
XYZ vVec = Cross(wVec, uVec);
vVec.Normalize();
//U is unit vec perpendicular to both V and W
uVec = Cross(vVec, wVec);
growthToWorld.BasisRotation(uVec, vVec, wVec);
worldToGrowth = growthToWorld.Inverse();
basisPoint = tCoords.Get(p1);
}
int Confo_Back::Recon_Back_WS_One(XYZ *mol,char *cle,int moln,XYZ **output,XYZ pre,XYZ nxt)
{
if(moln!=1)return -1;
XYZ real;
double dist;
//pre
real=pre-mol[0];
dist=pre.distance(mol[0]);
real=real/dist;
real=real*3.8;
case_mol[0]=mol[0]+real;
//nxt
real=nxt-mol[0];
dist=nxt.distance(mol[0]);
real=real/dist;
real=real*3.8;
case_mol[2]=mol[0]+real;
//construct
case_mol[1]=mol[0];
case_cle[0]='Q';
if(cle[0]!='R')case_cle[1]=cle[0];
else case_cle[1]='Q';
case_cle[2]='Q';
//build
int retv;
retv=Recon_Back_WS(case_mol,case_cle,3,case_mcb);
if(retv!=1)return retv;
//final
int j;
for(j=0;j<5;j++)output[0][j]=case_mcb[1][j];
//return
return 1;
}
void Confo_Back::Construct_Mol_II(XYZ pre,XYZ cur,XYZ nxt,double bend,double tort,double dist,XYZ &out)
{
XYZ xyz;
double x[3],y[3];
double z[3];
double cb1[3],cb2[3];
xyz=nxt-cur;
xyz.xyz2double(x);
xyz=cur-pre;
xyz.xyz2double(y);
//check
double angle=Vector_Angle(x,y,3);
if(fabs(angle)<1.e-3||fabs(angle-M_PI)<1.e-3)
{
x[0]+=0.05;
x[2]-=0.05;
y[0]-=0.05;
y[2]+=0.05;
}
//calc
cross(z,x,y);
Universal_Rotation(z,bend,Confo_Back_ROT_TOT);
Vector_Multiply(cb1,Confo_Back_ROT_TOT,x);
Universal_Rotation(x,tort,Confo_Back_ROT_TOT);
Vector_Multiply(cb2,Confo_Back_ROT_TOT,cb1);
Vector_Normalize(cb2,3);
Vector_Dot(cb2,dist,cb2,3);
out.double2xyz(cb2);
out+=nxt;
}
void SensorfwGyroscope::slotDataAvailable(const XYZ& data)
{
m_reading.setX((qreal)(data.x()*MILLI));
m_reading.setY((qreal)(data.y()*MILLI));
m_reading.setZ((qreal)(data.z()*MILLI));
m_reading.setTimestamp(data.XYZData().timestamp_);
newReadingAvailable();
}
void meegorotationsensor::slotDataAvailable(const XYZ& data)
{
m_reading.setX(data.x());
m_reading.setY(data.y());
m_reading.setZ(data.z());
m_reading.setTimestamp(data.XYZData().timestamp_);
newReadingAvailable();
}
int main(){
XYZ X;
ABC A;
X.setValue(20);
A.setValue(12);
add(X,A);
return 0;
}
void Transform::apply_index(Model *m, ParticleIndex pi) const {
if (!XYZ::get_is_setup(m, pi)) {
IMP_INTERNAL_CHECK(ignore_non_xyz_,
"The particle does not have XYZ attributes");
return;
}
XYZ xyz = XYZ(m, pi);
xyz.set_coordinates(t_.get_transformed(xyz.get_coordinates()));
}
void TrialMol::OldThetaAndPhi(const uint atom, const uint lastAtom,
double& theta, double& phi) const
{
XYZ diff = tCoords.Difference(atom, lastAtom);
diff = axes->MinImage(diff, box);
XYZ growthCoords = worldToGrowth.Apply(diff);
theta = acos(growthCoords.z / growthCoords.Length());
phi = atan2(growthCoords.y, growthCoords.x);
return;
}
void Transform::apply(Particle *p) const
{
if (!XYZ::particle_is_instance(p)) {
IMP_INTERNAL_CHECK(ignore_non_xyz_,
"The particle does not have XYZ attributes");
return;
}
XYZ xyz = XYZ(p);
xyz.set_coordinates(t_.get_transformed(xyz.get_coordinates()));
}
void meegoaccelerometer::slotDataAvailable(const XYZ& data)
{
// Convert from milli-Gs to meters per second per second
// Using 1 G = 9.80665 m/s^2
m_reading.setX(-data.x() * GRAVITY_EARTH_THOUSANDTH);
m_reading.setY(-data.y() * GRAVITY_EARTH_THOUSANDTH);
m_reading.setZ(-data.z() * GRAVITY_EARTH_THOUSANDTH);
m_reading.setTimestamp(data.XYZData().timestamp_);
newReadingAvailable();
}
void FDice::create(const XYZ& p0, const XYZ& p1, const XYZ& p2)
{
p_obj[0] = P[0] = p0;
p_obj[1] = P[1] = p1;
p_obj[2] = P[2] = p2;
V[0] = p1 - p0;
edge_length[0] = V[0].length();
V[1] = p2 - p1;
edge_length[1] = V[1].length();
V[2] = p0 - p2;
edge_length[2] = V[2].length();
area = 0.5*sqrt(edge_length[0]*edge_length[0]*edge_length[2]*edge_length[2] - (V[0].dot(V[2]))*(V[0].dot(V[2])));
V[0].normalize();
V[1].normalize();
V[2].normalize();
int a , b;
if(edge_length[0] > edge_length[1] && edge_length[0] > edge_length[2])
{
a = 0;
b = 1;
}
else if(edge_length[1] > edge_length[2] && edge_length[1] > edge_length[0])
{
a = 1;
b = 2;
}
else
{
a = 2;
b = 0;
}
XYZ side = V[a];
XYZ front = side^V[b]; front.normalize();
XYZ up = front^side;
m_space.setOrientations(side, up, front);
m_space.setTranslation(P[a]);
m_space.inverse();
m_space.transform(p_obj[0]);
m_space.transform(p_obj[1]);
m_space.transform(p_obj[2]);
f120 = barycentric_coord(p_obj[1].x, p_obj[1].y, p_obj[2].x, p_obj[2].y, p_obj[0].x, p_obj[0].y);
f201 = barycentric_coord(p_obj[2].x, p_obj[2].y, p_obj[0].x, p_obj[0].y, p_obj[1].x, p_obj[1].y);
f012 = barycentric_coord(p_obj[0].x, p_obj[0].y, p_obj[1].x, p_obj[1].y, p_obj[2].x, p_obj[2].y);
}
int Confo_Back::Recon_Back_WS_2nxt(XYZ *mol,char *cle,int moln,XYZ **output,XYZ nxt)
{
if(moln!=2)return -1;
XYZ real;
double dist;
real=nxt-mol[1];
dist=nxt.distance(mol[1]);
real=real/dist;
real=real*3.8;
real=mol[1]+real;
//construct
case_mol[0]=mol[0];
case_mol[1]=mol[1];
case_mol[2]=real;
if(cle[0]!='R')case_cle[0]=cle[0];
else case_cle[0]='Q';
if(cle[1]!='R')case_cle[1]=cle[1];
else case_cle[1]='Q';
case_cle[2]='Q';
//build
int retv;
retv=Recon_Back_WS(case_mol,case_cle,3,case_mcb);
if(retv!=1)return retv;
//final
int i,j;
for(i=0;i<2;i++)for(j=0;j<5;j++)output[i][j]=case_mcb[i][j];
//return
return 1;
}
char sphericalHarmonics::isCloseTo(const XYZ& ray, const float threshold, unsigned int id) const
{
SHSample& s = getSample(id);
float c = ray.dot(s.vector);
float t = sqrt(1-c*c)/c;
if(t >threshold) return 0;
return 1;
}
//--------------function------------//
//given CA+CB -> return dist,bend,tort
void Confo_Beta::Confo_Beta_CACB_To_Angle(XYZ *CA,XYZ *CB,int moln,double *bend,double *tort,double *dist)
{
int i;
double r,b,t;
XYZ xyz;
//process
for(i=1;i<moln-1;i++)
{
//get_point
xyz=(CA[i]-CA[i-1]);
xyz.xyz2double(beta_v1);
xyz=(CA[i+1]-CA[i]);
xyz.xyz2double(beta_v2);
xyz=(CB[i]-CA[i]);
xyz.xyz2double(beta_y);
//calc
r=dot(beta_y,beta_y);
if(r<1.e-3)
{
r=0.0;
b=0.0;
t=0.0;
}
else
{
//calc_bend
b=Vector_Angle(beta_v1,beta_y,3);
//calc_tort
cross(beta_x1,beta_v1,beta_v2);
cross(beta_x2,beta_v1,beta_y);
t=Vector_Angle(beta_x1,beta_x2,3);
if(dot(beta_x1,beta_y)<0.0)t*=-1.0;
}
//evaluate
if(dist!=NULL)dist[i]=r;
if(bend!=NULL)bend[i]=b;
if(tort!=NULL)tort[i]=t;
}
//assign head_tail
if(dist!=NULL)dist[0]=-1.0;
if(bend!=NULL)bend[0]=0.0;
if(tort!=NULL)tort[0]=0.0;
if(dist!=NULL)dist[moln-1]=-1.0;
if(bend!=NULL)bend[moln-1]=0.0;
if(tort!=NULL)tort[moln-1]=0.0;
}
void TrialMol::SetBasis(const uint p1, const uint p2, const uint p3)
{
using namespace geom;
//W is unit vec of p1->p2
XYZ wVec = axes->MinImage(tCoords.Difference(p2, p1), box);
wVec.Normalize();
//U will be unit projection of p2->p3 onto plane normal to W
XYZ uVec = axes->MinImage(tCoords.Difference(p3, p2), box);
//V is unit vec perpendicular to both W and U
XYZ vVec = Cross(wVec, uVec);
vVec.Normalize();
//Finish X'
uVec = Cross(vVec, wVec);
growthToWorld.BasisRotation(uVec, vVec, wVec);
worldToGrowth = growthToWorld.Inverse();
basisPoint = tCoords.Get(p1);
}
//given CA and dist,bend,tort -> return CB (Zheng & Liu's method)
void Confo_Beta::Confo_Beta_Angle_To_CACB(XYZ *CA,XYZ *CB,int moln,double *bend,double *tort,double *dist)
{
int i,j;
XYZ xyz;
double beta,radii;
//process
for(i=1;i<moln-1;i++)
{
//init
if(dist==NULL)beta=Confo_Beta_Levit_Radii;
else beta=dist[i];
if(beta<0.0)
{
CB[i]=CA[i];
continue;
}
//calc
xyz=(CA[i]-CA[i-1]);
xyz.xyz2double(beta_v1);
xyz=(CA[i+1]-CA[i]);
xyz.xyz2double(beta_v2);
cross(beta_y,beta_v1,beta_v2);
Universal_Rotation(beta_y,bend[i],Confo_Beta_rotmat);
Vector_Multiply(beta_x1,Confo_Beta_rotmat,beta_v1);
Universal_Rotation(beta_v1,tort[i],Confo_Beta_rotmat);
Vector_Multiply(beta_x2,Confo_Beta_rotmat,beta_x1);
//evaluate
radii=sqrt(dot(beta_x2,beta_x2));
CA[i].xyz2double(beta_ca);
for(j=0;j<3;j++)beta_cb[j]=beta_ca[j]+beta*beta_x2[j]/radii;
CB[i].double2xyz(beta_cb);
}
//assign head_tail //this might be modified in the future...
if(moln>1)
{
CB[0]=CB[1]-CA[1]+CA[0];
CB[moln-1]=CB[moln-2]-CA[moln-2]+CA[moln-1];
}
else
{
CB[0]=CA[0];
CB[moln-1]=CA[moln-1];
}
}
//given CA -> return CB (Levitt's method)
void Confo_Beta::Confo_Beta_Levit_To_CACB(XYZ *CA,XYZ *CB,int moln,double *dist)
{
int i,j;
XYZ xyz;
double beta,theta;
//process
theta=Confo_Beta_Levit_Angle;
for(i=1;i<moln-1;i++) //omit head and tail
{
//init
if(dist==NULL)beta=Confo_Beta_Levit_Radii;
else beta=dist[i];
if(beta<0.0)
{
CB[i]=CA[i];
continue;
}
//calc
xyz=(CA[i]-CA[i-1]);
xyz.xyz2double(beta_v1);
xyz=(CA[i]-CA[i+1]);
xyz.xyz2double(beta_v2);
Vector_Addition(beta_x1,beta_v1,beta_v2,3);
Vector_Normalize(beta_x1,3);
cross(beta_x2,beta_v1,beta_v2);
Vector_Normalize(beta_x2,3);
//evaluate
CA[i].xyz2double(beta_ca);
for(j=0;j<3;j++)beta_cb[j]=beta_ca[j]+beta*(cos(theta)*beta_x1[j]+sin(theta)*beta_x2[j]);
CB[i].double2xyz(beta_cb);
}
//assign head_tail //this might be modified in the future...
if(moln>1)
{
CB[0]=CB[1]-CA[1]+CA[0];
CB[moln-1]=CB[moln-2]-CA[moln-2]+CA[moln-1];
}
else
{
CB[0]=CA[0];
CB[moln-1]=CA[moln-1];
}
}
void TriangleMeshViewer::tick()
{
const int t=clock->elapsed();
const float dt=0.001f*(t-last_t);
last_t=t;
camera_roll_rate=0.0f;
if (keypressed_arrow_left || keypressed_mouse_left) camera_roll_rate+=0.5f;
if (keypressed_arrow_right || keypressed_mouse_right) camera_roll_rate-=0.5f;
if (keypressed_arrow_up) camera_velocity+=120.0f*(0.03125f/480.0f);
if (keypressed_arrow_down) camera_velocity-=120.0f*(0.03125f/480.0f);
//! \todo Replace cheesy rotation hacks with proper rotation matrices
XYZ camera_right=(camera_forward*camera_up).normalised();
const XYZ camera_right_rolled=camera_right+(dt*camera_roll_rate)*camera_up;
const XYZ camera_up_rolled=camera_up-(dt*camera_roll_rate)*camera_right;
camera_right=camera_right_rolled.normalised();
camera_up=camera_up_rolled.normalised();
camera_forward=(camera_forward+dt*camera_yaw_rate*camera_right+dt*camera_pitch_rate*camera_up).normalised();
camera_up=(camera_right*camera_forward).normalised();
camera_position+=(dt*camera_velocity)*camera_forward;
object_rotation+=object_spinrate*dt;
if (display->isVisible())
{
display->draw_frame(camera_position,camera_position+camera_forward,camera_up,object_rotation,object_tilt);
}
std::ostringstream msg;
if (fly_mode)
{
msg << "Velocity:" << camera_velocity << " Roll rate:" << camera_roll_rate << " ";
}
msg << notify_message;
statusbar->showMessage(msg.str().c_str());
}
void Confo_Back::Construct_CB(XYZ N,XYZ Ca,XYZ C,double bend,double tort,double dist,XYZ &Cb)
{
XYZ xyz;
double x[3],y[3];
double z[3];
double cb1[3],cb2[3];
xyz=C-Ca;
xyz.xyz2double(x);
xyz=Ca-N;
xyz.xyz2double(y);
cross(z,x,y);
Universal_Rotation(z,-1.0*bend,Confo_Back_ROT_TOT);
Vector_Multiply(cb1,Confo_Back_ROT_TOT,x);
Universal_Rotation(x,-1.0*tort,Confo_Back_ROT_TOT);
Vector_Multiply(cb2,Confo_Back_ROT_TOT,cb1);
Vector_Normalize(cb2,3);
Vector_Dot(cb2,dist,cb2,3);
Cb.double2xyz(cb2);
Cb+=Ca;
}
void Console::doCompute(){
// Matrix3 CAM = chromaticAdaptationMatrix(SRC_RW, DST_RW, METHOD);
// XYZ.from(SRC, SRC_RW);
// XYZ = CAM * XYZ
// SRC.from(XYZ, DST_RW);
// SRC
int srwIndex = _srwIndex + _srcObsIndex;
int drwIndex = _drwIndex + _dstObsIndex;
const Illuminant* srw = nullptr;
const Illuminant* drw = nullptr;
XYZ xyz;
Vector3 tri;
tri(0) = get<1>(_input)->text().toFloat();
tri(1) = get<2>(_input)->text().toFloat();
tri(2) = get<3>(_input)->text().toFloat();
if(_fromIndex >= 0 and _fromIndex <= 5)
srw = _rw[srwIndex];
else
_cs[_fromIndex]->referenceWhite(srw);
if(_toIndex >= 0 and _toIndex <= 5)
drw = _rw[drwIndex];
else
_cs[_toIndex]->referenceWhite(drw);
_cs[_fromIndex]->coords() = tri;
xyz.coords() = _cs[_fromIndex]->to_XYZ(srw);
Matrix3 cam = chromaticAdaptationMatrix(*srw, *drw, _am[_camIndex]);
xyz.coords() = cam * xyz.coords();
_cs[_toIndex]->from_XYZ(xyz.coords(), drw);
tri = _cs[_toIndex]->coords();
get<1>(_output)->setText(QString::number( tri(0), 'f', 10));
get<2>(_output)->setText(QString::number( tri(1), 'f', 10));
get<3>(_output)->setText(QString::number( tri(2), 'f', 10));
}
//===================== Recon_Beta ==================//
//given CA+CB, and AMI_Dist, update SC
void Confo_Beta::Recon_Beta_1(XYZ *CA,XYZ *CB,int moln,char *ami) //return SC
{
int i;
XYZ xyz;
double radii;
double multi;
double v[3];
for(i=0;i<moln;i++)
{
xyz=(CB[i]-CA[i]);
xyz.xyz2double(v);
radii=dot(v,v);
if(radii<1.e-3)continue;
Vector_Normalize(v,3);
multi=CB_AMI_Dist[ami[i]-'A'];
if(multi<0.0)continue;
Vector_Dot(v,multi,v,3);
xyz.double2xyz(v);
CB[i]=CA[i]+xyz;
}
}
void FNoiseVolume::getSideAndUpById(unsigned id, XYZ& side, XYZ& up) const
{
side = XYZ(1,0,0);
up = XYZ(0,1,0);
XYZ zz(0,0,1);
side.rotateXY(data[id].noi*6.28);
up = zz.cross(side);
eyespace.transformAsNormal(side);
eyespace.transformAsNormal(up);
side *= global_size;
up *= global_size;
}
void sphericalHarmonics::bDiffuseTransfer(SHSample* samp, const int num_samp, const float unitarea, XYZ* inCoeff, XYZ* outCoeff, const XYZ& normal) const
{
bReconstructSH rsh(inCoeff);
for (int j = 0; j < SH_N_BASES; j++) outCoeff[j] = XYZ(0);
for(int j=0; j<num_samp; j++)
{
SHSample& s = samp[j];
float H = normal.dot(s.vector);
if(H>0)
{
XYZ col = rsh.getColor(s) * H * unitarea;
projectASample(outCoeff, s, col);
}
}
}
void sphericalHarmonics::diffuseTransfer(SHSample* samp, const int num_samp, const float unitarea, XYZ* inCoeff, XYZ* outCoeff, float* tmpCoeff, const XYZ& normal) const
{
reconstructSH rsh(inCoeff);
for (int j = 0; j < SH_N_BASES; j++)
tmpCoeff[j] = 0;
for(int j=0; j<num_samp; j++)
{
SHSample& s = samp[j];
float H = normal.dot(s.vector);
if(H>0)
{
float shd = rsh.getFloat(s) * H * unitarea;
projectASample(tmpCoeff, s, shd);
}
}
for (int j = 0; j < SH_N_BASES; j++)
outCoeff[j].x = outCoeff[j].y = outCoeff[j].z = tmpCoeff[j];
}
// rotar p con eje de rotacion r
XYZ rotate(XYZ p, XYZ r, double theta){
XYZ q(0,0,0);
double costheta,sintheta;
r.normalize();
costheta = cos(theta);
sintheta = sin(theta);
q.x += (costheta + (1 - costheta) * r.x * r.x) * p.x;
q.x += ((1 - costheta) * r.x * r.y - r.z * sintheta) * p.y;
q.x += ((1 - costheta) * r.x * r.z + r.y * sintheta) * p.z;
q.y += ((1 - costheta) * r.x * r.y + r.z * sintheta) * p.x;
q.y += (costheta + (1 - costheta) * r.y * r.y) * p.y;
q.y += ((1 - costheta) * r.y * r.z - r.x * sintheta) * p.z;
q.z += ((1 - costheta) * r.x * r.z - r.y * sintheta) * p.x;
q.z += ((1 - costheta) * r.y * r.z + r.x * sintheta) * p.y;
q.z += (costheta + (1 - costheta) * r.z * r.z) * p.z;
return q;
}
//input Confo_Angle (*dist,*bend,*tort), output XYZ_Type data_structure (*r)
//dist[0] : -1.0;
//dist[1]-[n]: normal
//bend[0]-[1]: -9.9
//bend[2]-[n]: normal
//tort[0]-[2]: -9.9
//tort[3]-[n]: normal
void Confo_Lett::ctc_ori(double *dist,double *bend,double *tort,int n,XYZ *r,XYZ *init)
{
int i;
double radi;
XYZ xyz;
xyz=0.0;
XYZ ori[3],pos[3];
//real_process//
Matrix_Unit(cle_T_Back,1.0);
for(i=0;i<n;i++)
{
if(dist==NULL)radi=3.8;
else radi=dist[i];
if(i==0)r[i]=0.0;
else if(i==1)
{
r[i]=0.0;
r[i].X=radi;
}
else if(i==2)
{
Universal_Rotation(cle_Z_Axis,bend[i],cle_R_Theta);
Vector_Dot(cle_D_Back,radi,cle_X_Axis,3);
Matrix_Multiply(cle_T_Pre,cle_T_Back,cle_R_Theta);
Vector_Multiply(cle_D_Pre,cle_T_Pre,cle_D_Back);
Matrix_Equal(cle_T_Back,cle_T_Pre);
xyz.double2xyz(cle_D_Pre);
r[i]=r[i-1]+xyz;
}
else
{
Universal_Rotation(cle_Z_Axis,bend[i],cle_R_Theta);
Universal_Rotation(cle_X_Axis,tort[i],cle_R_Thor);
Vector_Dot(cle_D_Back,radi,cle_X_Axis,3);
Matrix_Multiply(cle_temp,cle_R_Thor,cle_R_Theta);
Matrix_Multiply(cle_T_Pre,cle_T_Back,cle_temp);
Vector_Multiply(cle_D_Pre,cle_T_Pre,cle_D_Back);
Matrix_Equal(cle_T_Back,cle_T_Pre);
xyz.double2xyz(cle_D_Pre);
r[i]=r[i-1]+xyz;
}
}
//whether do ini_vector
if(init!=NULL && n>=3)
{
for(i=0;i<3;i++)
{
ori[i]=init[i];
pos[i]=r[i];
}
Get_Initial(ori,pos,cle_r1,cle_r2);
Matrix_Multiply(cle_T_Pre,cle_r2,cle_r1);
for(i=0;i<n;i++)
{
r[i].xyz2double(cle_D_Back);
Vector_Multiply(cle_D_Pre,cle_T_Pre,cle_D_Back);
xyz.double2xyz(cle_D_Pre);
r[i]=ori[0]+xyz;
}
}
}
void SensorfwRotationSensor::slotDataAvailable(const XYZ& data)
{
m_reading.setFromEuler(data.x(),data.y(),data.z());
m_reading.setTimestamp(data.XYZData().timestamp_);
newReadingAvailable();
}
void transform(XYZ a, const algebra::Transformation3D &tr) {
IMP_USAGE_CHECK(!RigidBody::particle_is_instance(a),
"Python is calling the wrong function");
a.set_coordinates(tr.get_transformed(a.get_coordinates()));
}
void HairCache::bind()
{
if(!guide_data || n_samp < 1) return;
if(bind_data) delete[] bind_data;
bind_data = new triangle_bind_info[n_samp];
if(pNSeg) delete[] pNSeg;
pNSeg = new unsigned[n_samp];
for(unsigned i=0; i<n_samp; i++)
{
// restrict st between 0 - 1
if(ddice[i].coords < 0) ddice[i].coords = 0;
if(ddice[i].coords > 1) ddice[i].coords = 1;
if(ddice[i].coordt < 0) ddice[i].coordt = 0;
if(ddice[i].coordt > 1) ddice[i].coordt = 1;
// find the nearest guide
XY from(ddice[i].coords, ddice[i].coordt);
ValueAndId* idx = new ValueAndId[num_guide];
for(unsigned j=0; j<num_guide; j++)
{
idx[j].idx = j;
XY to(guide_data[j].u, guide_data[j].v);
idx[j].val = from.distantTo(to);
}
QuickSort::sort(idx, 0, num_guide-1);
// find closest guider valid in 3D
unsigned validid = 0;
XYZ pto = pBind[ddice[i].id0]*ddice[i].alpha + pBind[ddice[i].id1]*ddice[i].beta + pBind[ddice[i].id2]*ddice[i].gamma;
XYZ dto = pto - guide_data[idx[validid].idx].P[0];
float dist2valid = dto.length();
while(dist2valid > guide_data[idx[validid].idx].radius*2 && validid<num_guide-1) {
validid++;
dto = pto - guide_data[idx[validid].idx].P[0];
dist2valid = dto.length();
}
bind_data[i].wei[0] = 1.f;
bind_data[i].wei[1] = bind_data[i].wei[2] = 0.f;
bind_data[i].idx[0] = bind_data[i].idx[1] = bind_data[i].idx[2] = idx[validid].idx;
if(validid < num_guide-6) {
XY corner[3];XYZ pw[3]; float dist[3];
for(unsigned hdl=0; hdl<3; hdl++) {
bind_data[i].idx[0] = idx[validid].idx;
bind_data[i].idx[1] = idx[validid+1+hdl].idx;
bind_data[i].idx[2] = idx[validid+2+hdl].idx;
corner[0].x = guide_data[idx[validid].idx].u;
corner[1].x = guide_data[idx[validid+1+hdl].idx].u;
corner[2].x = guide_data[idx[validid+2+hdl].idx].u;
corner[0].y = guide_data[idx[validid].idx].v;
corner[1].y = guide_data[idx[validid+1+hdl].idx].v;
corner[2].y = guide_data[idx[validid+2+hdl].idx].v;
pw[0] = guide_data[idx[validid].idx].P[0];
pw[1] = guide_data[idx[validid+1+hdl].idx].P[0];
pw[2] = guide_data[idx[validid+2+hdl].idx].P[0];
dist[0] = guide_data[idx[validid].idx].radius;
dist[1] = guide_data[idx[validid+1+hdl].idx].radius;
dist[2] = guide_data[idx[validid+2+hdl].idx].radius;
if( BindTriangle::set(corner, from, pw, pto, dist, bind_data[i]) ) hdl = 4;
else bind_data[i].idx[0] = bind_data[i].idx[1] = bind_data[i].idx[2] = idx[validid].idx;
}
}
pNSeg[i] = guide_data[bind_data[i].idx[0]].num_seg;// * bind_data[i].wei[0] + guide_data[bind_data[i].idx[1]].num_seg * bind_data[i].wei[1] + guide_data[bind_data[i].idx[2]].num_seg * bind_data[i].wei[2];
delete[] idx;
}
}