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;
}
//-----------------------//---------------------------------------------------//
//----Confo_Lett->XYZ---//
//---------------------//
void Confo_Lett::Get_Initial(XYZ *in1,XYZ *in2,double r1[3][3],double r2[3][3])
{
int i;
double phi;
double thi;
//--get_first_rotation--//
(in1[1]-in1[0]).xyz2double(cle_v1); //v1 -> ori_coordinate
(in2[1]-in2[0]).xyz2double(cle_v2); //v2 -> pos_coordinate
phi=dot(cle_v1,cle_v2)/sqrt(dot(cle_v1,cle_v1)*dot(cle_v2,cle_v2));
phi=limit(phi,-1.0,1.0);
phi=acos(1.0*phi);
cross(cle_Axis,cle_v2,cle_v1);
Universal_Rotation(cle_Axis,phi,r1);
//--get_second_rotation--//
(in1[2]-in1[0]).xyz2double(cle_w1); //w1 -> ori_coordinate
(in2[2]-in2[0]).xyz2double(cle_w2); //w2 -> pos_coordinate
Vector_Multiply(cle_test,r1,cle_w2);
for(i=0;i<3;i++)cle_w2[i]=cle_test[i];
cross(cle_x1,cle_v1,cle_w2);
cross(cle_x2,cle_v1,cle_w1);
cross(cle_test,cle_x1,cle_x2);
thi=dot(cle_x1,cle_x2)/sqrt(dot(cle_x1,cle_x1)*dot(cle_x2,cle_x2));
thi=limit(thi,-1.0,1.0);
thi=acos(1.0*thi);
if(dot(cle_test,cle_v1)<0.0)thi*=-1.0;
Universal_Rotation(cle_v1,thi,r2);
}
//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];
}
}
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;
}
//
// A spring dampening function
// for the camera
//
void SpringDamp(
Vector currPos,
Vector trgPos, // Target Position
Vector prevTrgPos, // Previous Target Position
Vector result,
float springConst, // Hooke's Constant
float dampConst, // Damp Constant
float springLen)
{
Vector disp; // Displacement
Vector velocity; // Velocity
Vector mx;
Vector z;
float len;
float dot;
float forceMag; // Force Magnitude
// Calculate Spring Force
Vector_Minus(currPos, trgPos, disp);
Vector_Minus(prevTrgPos, trgPos, velocity);
len = Vector_Length(disp);
// get dot product
dot = DotProduct(disp, velocity);
forceMag = springConst * (springLen - len) +
dampConst * (dot / len);
Vector_Normalize(disp, z);
//disp *= forceMag;
Vector_Multiply(z, mx, forceMag);
printf("%0.2f %0.2f\n", mx[0], currPos[0]);
Vector_Add(currPos, mx, result);
//result[0] = currPos[0];
//result[1] = currPos[1];
//result[2] = currPos[2];
} // end of the function
//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;
}
}
}
