ICODE-ADC/lammps
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
005433c7e300239b51bd3100e5781bff44326d16
lammps/tools/pymol_asphere/src/cartesian.cpp
Eisuke Kawashima 649a8cc01a Fix typo
2020-03-14 13:57:48 +09:00

1048 lines
24 KiB
C++
Raw Blame History

/****************************************************************************
* cartesian.cpp
* Shared stuff for dealing with Cartesian coordinates
* Also contains quaternion structures and operations
*
* Cartesian point of doubles: cPt
* Cartesian point of integers: iPt
* Vector of doubles: vectorPt
* Color of doubles: colorPt
* Quaternion: Quaternion
*
* Can be accessed as cPt.x, cPt[X], colorPt[GREEN], iPt[I], etc.
*
*
* W. Michael Brown
* 5/26/03
****************************************************************************/
#include "cartesian.h"
// ------------------------ TwoD Stuff
template<class numtyp>
TwoD<numtyp>::TwoD() {
}
// Assign both x and y the value
template<class numtyp>
TwoD<numtyp>::TwoD(numtyp in) {
x=in;
y=in;
}
// Assignment Constructor
template<class numtyp>
TwoD<numtyp>::TwoD(numtyp cx, numtyp cy) {
x=cx;
y=cy;
}
// Type conversion
template<class numtyp>
TwoD<numtyp>::TwoD(const TwoD<float> &two) {
x=numtyp(two.x);
y=numtyp(two.y);
}
// Ball projection (onto xy-plane)
template<class numtyp>
TwoD<numtyp>::TwoD(const Ball<float> &ball) {
x=numtyp(cos(ball.theta));
y=numtyp(sin(ball.theta));
}
template<class numtyp>
numtyp & TwoD<numtyp>::operator[](unsigned i) {
switch(i) {
case X: return x;
case Y: return y;
}
return x;
}
template<class numtyp>
TwoD<numtyp> TwoD<numtyp>::operator + (const TwoD<numtyp> &two) const {
return TwoD(x+two.x, y+two.y);
}
template<class numtyp>
void TwoD<numtyp>::operator += (const TwoD<numtyp> &two) {
x+=two.x;
y+=two.y;
}
template<class numtyp>
TwoD<numtyp> TwoD<numtyp>::operator + (const numtyp two) const {
return TwoD(x+two, y+two);
}
template<class numtyp>
TwoD<numtyp> TwoD<numtyp>::operator - (const TwoD<numtyp> &two) const {
return TwoD(x-two.x, y-two.y);
}
template<class numtyp>
TwoD<numtyp> TwoD<numtyp>::operator - (const numtyp two) const {
return TwoD(x-two, y-two);
}
template<class numtyp>
TwoD<numtyp> TwoD<numtyp>::operator * (const numtyp two) const {
return TwoD(x*two, y*two);
}
template<class numtyp>
TwoD<numtyp> TwoD<numtyp>::operator * (const TwoD<numtyp> &two) const {
return TwoD<numtyp>(x*two.x,y*two.y);
}
template<class numtyp>
void TwoD<numtyp>::operator /= (const numtyp two) {
x/=two;
y/=two;
}
template<class numtyp>
bool TwoD<numtyp>::operator != (const TwoD<numtyp> &two) const {
if (x!=two.x || y!=two.y)
return true;
return false;
}
// Move coordinates into array
template<class numtyp>
void TwoD<numtyp>::to_array(numtyp *array) {
*array=x;
array++;
*array=y;
}
// Set coordinates from array
template<class numtyp>
void TwoD<numtyp>::from_array(numtyp *array) {
x=*array;
array++;
y=*array;
}
// Dot Product
template<class numtyp>
numtyp TwoD<numtyp>::dot(const TwoD<numtyp> &two) const {
return x*two.x+y*two.y;
}
// Returns one of two normals to a line represented by vector
template<class numtyp>
TwoD<numtyp> TwoD<numtyp>::normal() {
return TwoD(y,x*numtyp(-1));
}
template<class numtyp>
numtyp TwoD<numtyp>::dist2(const TwoD<numtyp> &two) const {
TwoD diff=*this-two;
return diff.dot(diff);
}
// Returns two
template<class numtyp>
unsigned TwoD<numtyp>::dimensionality() {
return 2;
}
// Returns the index of *this (for unsigned) in a square 3D array
/* s_size[0]=1Dsize */
template<class numtyp>
numtyp TwoD<numtyp>::array_index(vector<unsigned> &s_size) {
return y+x*s_size[0];
}
// Increment a 3D index from min to max (same as nested for)
template <>
bool TwoD<unsigned>::increment_index(TwoD &minp,TwoD &maxp) {
x++;
if (x!=maxp.x)
return true;
x=minp.x;
y++;
if (y!=maxp.y)
return true;
return false;
}
// Return false if x or y is not within the inclusive range
template<class numtyp>
bool TwoD<numtyp>::check_bounds(numtyp minp,numtyp maxp) {
if (x<minp || y<minp || x>maxp || y>maxp)
return false;
return true;
}
template<class numtyp>
ostream & operator<<(ostream &out, const TwoD<numtyp> &t) {
out << t.x << " " << t.y;
return out;
}
template<class numtyp>
istream & operator>>(istream &in, TwoD<numtyp> &t) {
in >> t.x >> t.y;
return in;
}
// ------------------------ ThreeD Stuff
// Assignment Constructor
template<class numtyp>
ThreeD<numtyp>::ThreeD(numtyp cx, numtyp cy, numtyp cz) {
x=cx; y=cy; z=cz;
}
// Assignment Constructor
template<class numtyp>
ThreeD<numtyp>::ThreeD(numtyp in) {
x=in; y=in; z=in;
}
// Type Conversion
template<class numtyp>
ThreeD<numtyp>::ThreeD(const ThreeD<unsigned>& in) {
x=numtyp(in.x); y=numtyp(in.y); z=numtyp(in.z);
}
// Type Conversion
template<class numtyp>
ThreeD<numtyp>::ThreeD(const ThreeD<int>& in) {
x=numtyp(in.x); y=numtyp(in.y); z=numtyp(in.z);
}
// Type Conversion
template<class numtyp>
ThreeD<numtyp>::ThreeD(const ThreeD<float>& in) {
x=numtyp(in.x); y=numtyp(in.y); z=numtyp(in.z);
}
// Type Conversion
template<class numtyp>
ThreeD<numtyp>::ThreeD(const ThreeD<double>& in) {
x=numtyp(in.x); y=numtyp(in.y); z=numtyp(in.z);
}
// TwoD with (z-coordinate set to zero)
template<class numtyp>
ThreeD<numtyp>::ThreeD(const TwoD<float>& in) {
x=numtyp(in.x); y=numtyp(in.y); z=0;
}
// Spherical Conversion
template<class numtyp>
ThreeD<numtyp>::ThreeD(const Ball<double>& ball) {
double sp=sin(ball.phi);
x=numtyp(sp*cos(ball.theta));
y=numtyp(sp*sin(ball.theta));
z=numtyp(cos(ball.phi));
}
// Spherical Conversion
template<class numtyp>
ThreeD<numtyp>::ThreeD(const Ball<float>& ball) {
double sp=sin(ball.phi);
x=numtyp(sp*cos(ball.theta));
y=numtyp(sp*sin(ball.theta));
z=numtyp(cos(ball.phi));
}
template<class numtyp>
ThreeD<numtyp>::ThreeD() {
}
template<class numtyp>
void ThreeD<numtyp>::operator = (const ThreeD &two) {
#ifdef NANCHECK
//assert(a::not_nan(two.x) && a::not_nan(two.y) && a::not_nan(two.z));
#endif
x=two.x; y=two.y; z=two.z;
}
template<class numtyp>
bool ThreeD<numtyp>::operator == (const ThreeD<numtyp> &two) const {
if (x!=two.x || y!=two.y || z!=two.z)
return false;
return true;
}
template<class numtyp>
bool ThreeD<numtyp>::operator != (const ThreeD<numtyp> &two) const {
if (x!=two.x || y!=two.y || z!=two.z)
return true;
return false;
}
template<class numtyp>
ThreeD<numtyp> ThreeD<numtyp>::operator + (const ThreeD<numtyp> &two) const {
return (ThreeD<numtyp>(x+two.x,y+two.y,z+two.z));
}
template <class numtyp>
ThreeD<numtyp> ThreeD<numtyp>::operator + (const numtyp &two) const {
return (ThreeD<numtyp>(x+two,y+two,z+two));
}
template<class numtyp>
void ThreeD<numtyp>::operator += (const numtyp &two) {
x+=two; y+=two; z+=two;
}
template<class numtyp>
void ThreeD<numtyp>::operator += (const ThreeD &two) {
x+=two.x; y+=two.y; z+=two.z;
}
template<class numtyp>
void ThreeD<numtyp>::operator -= (const ThreeD &two) {
x-=two.x; y-=two.y; z-=two.z;
}
template <class numtyp>
ThreeD<numtyp> ThreeD<numtyp>::operator - (const ThreeD<numtyp> &two) const {
return (ThreeD<numtyp>(x-two.x,y-two.y,z-two.z));
}
template <class numtyp>
ThreeD<numtyp> ThreeD<numtyp>::operator - (const numtyp &two) const {
return (ThreeD<numtyp>(x-two,y-two,z-two));
}
template<class numtyp>
void ThreeD<numtyp>::operator -= (const numtyp &two) {
x-=two; y-=two; z-=two;
}
template <class numtyp>
ThreeD<numtyp> ThreeD<numtyp>::operator * (const numtyp &two) const {
return (ThreeD<numtyp>(x*two,y*two,z*two));
}
template <class numtyp>
ThreeD<numtyp> ThreeD<numtyp>::operator * (const ThreeD<numtyp> &two) const {
return ThreeD<numtyp>(x*two.x,y*two.y,z*two.z);
}
template <class numtyp>
ThreeD<numtyp> ThreeD<numtyp>::operator / (const ThreeD<numtyp> &two) const {
return ThreeD<numtyp>(x/two.x,y/two.y,z/two.z);
}
// Dot Product
template <class numtyp>
numtyp ThreeD<numtyp>::dot(const ThreeD<numtyp> &two) const {
return (x*two.x+y*two.y+z*two.z);
}
// Move coordinates into array
template<class numtyp>
void ThreeD<numtyp>::to_array(numtyp *array) {
*array=x;
array++;
*array=y;
array++;
*array=z;
}
// Set coordinates from array
template<class numtyp>
void ThreeD<numtyp>::from_array(numtyp *array) {
x=*array;
array++;
y=*array;
array++;
z=*array;
}
// Return the cross product of *this and two
template<class numtyp>
ThreeD<numtyp> ThreeD<numtyp>::cross(const ThreeD<numtyp> &two) const {
return ThreeD<numtyp> (y*two.z-z*two.y, z*two.x-x*two.z, x*two.y-y*two.x);
}
// Return the angle between two vectors
template<class numtyp>
numtyp ThreeD<numtyp>::angle(const ThreeD<numtyp> &two) const {
#ifdef NANCHECK
//assert(fabs(double((*this*two)/(norm()*two.norm())))<=1);
#endif
return(numtyp(acos( double(dot(two)/(norm()*two.norm())) )));
}
// Returns an arbitrary vector that is perpendicular to the input
template <class numtyp>
ThreeD<numtyp> ThreeD<numtyp>::perpendicular() {
ThreeD<numtyp> two=*this;
if (y==0 && z==0)
two.y+=1;
else
two.x+=1;
return cross(two);
}
template <class numtyp>
ThreeD<numtyp> ThreeD<numtyp>::rotatey(double t) const {
ThreeD<numtyp> two;
numtyp sint=numtyp(sin(t));
numtyp cost=numtyp(cos(t));
two.x=(x*cost)-(z*sint);
two.z=(x*sint)+(z*cost);
two.y=y;
return two;
}
template <class numtyp>
ThreeD<numtyp> ThreeD<numtyp>::rotatez(double t) const {
ThreeD<numtyp> two;
numtyp sint=numtyp(sin(t));
numtyp cost=numtyp(cos(t));
two.x=(x*cost)-(y*sint);
two.y=(x*sint)+(y*cost);
two.z=z;
return two;
}
template <class numtyp>
ThreeD<numtyp> ThreeD<numtyp>::operator / (const numtyp &two) const {
return (ThreeD<numtyp>(x/two,y/two,z/two));
}
template<class numtyp>
void ThreeD<numtyp>::operator /= (const numtyp &two) {
x/=two; y/=two; z/=two;
}
// Magnitude of vector
template <class numtyp>
numtyp ThreeD<numtyp>::hypot() const {
return norm();
}
// Return the norm of a vector
template<class numtyp>
numtyp ThreeD<numtyp>::norm() const {
numtyp xi=x*x; numtyp yi=y*y; numtyp zi=z*z;
return numtyp(sqrt(double(xi+yi+zi)));
}
// Return the squared norm of a vector
template<class numtyp>
numtyp ThreeD<numtyp>::norm2() const {
return x*x+y*y+z*z;
}
template <class numtyp>
numtyp ThreeD<numtyp>::dist2(const ThreeD<numtyp> &two) {
ThreeD diff=*this-two;
return diff.dot(diff);
}
// Return unit vector
template<class numtyp>
ThreeD<numtyp> ThreeD<numtyp>::unit() {
ThreeD<numtyp> unit=*this;
unit.normalize();
return unit;
}
/// Returns 3
template<class numtyp>
unsigned ThreeD<numtyp>::dimensionality() {
return 3;
}
template<class numtyp>
c2DPt ThreeD<numtyp>::projz() {
return c2DPt(x,y);
}
template<class numtyp>
ostream & operator<< (ostream &out, const ThreeD<numtyp> &t) {
out << t.x << " " << t.y << " " << t.z;
return out;
}
template<class numtyp>
istream & operator>>(istream &in, ThreeD<numtyp> &t) {
in >> t.x >> t.y >> t.z;
return in;
}
template<class numtyp>
ThreeD<numtyp> operator+ (const numtyp one, const ThreeD<numtyp> &two) {
return ThreeD<numtyp>(one+two.x,one+two.y,one+two.z);
}
template<class numtyp>
ThreeD<numtyp> operator- (const numtyp one, const ThreeD<numtyp> &two) {
return ThreeD<numtyp>(one-two.x,one-two.y,one-two.z);
}
template<class numtyp>
ThreeD<numtyp> operator* (const numtyp one, const ThreeD<numtyp> &two) {
return ThreeD<numtyp>(one*two.x,one*two.y,one*two.z);
}
template<class numtyp>
ThreeD<numtyp> operator/ (const numtyp one, const ThreeD<numtyp> &two) {
return ThreeD<numtyp>(one/two.x,one/two.y,one/two.z);
}
// Set this to be the max of one and two for each dimension
template<class numtyp>
void ThreeD<numtyp>::max(ThreeD<numtyp> &one, ThreeD<numtyp> &two) {
x=am::max(one.x,two.x);
y=am::max(one.y,two.y);
z=am::max(one.z,two.z);
}
// Set this to be the max of one and two for each dimension
template<class numtyp>
void ThreeD<numtyp>::min(ThreeD<numtyp> &one, ThreeD<numtyp> &two) {
x=am::min(one.x,two.x);
y=am::min(one.y,two.y);
z=am::min(one.z,two.z);
}
// Returns the index of *this (for unsigned) in a square 3D array
/* s_size[0]=1Dsize, and s_size[1]=1D size*1Dsize */
template<class numtyp>
numtyp ThreeD<numtyp>::array_index(vector<unsigned> &s_size) {
return z+y*s_size[0]+x*s_size[1];
}
// Increment a 3D index from min to max (same as nested for)
template <>
bool ThreeD<unsigned>::increment_index(ThreeD &minp,ThreeD &maxp) {
x++;
if (x!=maxp.x)
return true;
x=minp.x;
y++;
if (y!=maxp.y)
return true;
y=minp.y;
z++;
if (z!=maxp.z)
return true;
return false;
}
// Return false if x,y, or z is not within the inclusive range
template<class numtyp>
bool ThreeD<numtyp>::check_bounds(numtyp minp,numtyp maxp) {
if (x<minp || y<minp || z<minp || x>maxp || y>maxp || z>maxp)
return false;
return true;
}
// --------------------- Quaternion Stuff -----------------------
Quaternion::Quaternion() {
#ifdef NANCHECK
w=0; i=0; j=0; k=0;
#endif
}
Quaternion::~Quaternion() {
}
Quaternion::Quaternion(const Quaternion &two) {
#ifdef NANCHECK
//assert(a::not_nan(two.w) && a::not_nan(two.i) && a::not_nan(two.j) &&
// a::not_nan(two.k));
#endif
w=two.w; i=two.i; j=two.j; k=two.k;
}
Quaternion::Quaternion(double inw, double ini, double inj, double ink) {
#ifdef NANCHECK
//assert(a::not_nan(inw) && a::not_nan(ini) && a::not_nan(inj) &&
// a::not_nan(ink));
#endif
w=inw; i=ini; j=inj; k=ink;
}
// Set the quaterion according to axis-angle (must be a UNIT vector)
Quaternion::Quaternion(const vectorPt &v, double angle) {
double halfa=angle/2;
double sina=sin(halfa);
w=cos(halfa);
i=v.x*sina;
j=v.y*sina;
k=v.z*sina;
#ifdef NANCHECK
//assert(a::not_nan(w) && a::not_nan(i) && a::not_nan(j) && a::not_nan(k));
#endif
}
// Set the quaterion according to rotation from vector 1 to vector 2
// The input vectors do not need to be normalized
Quaternion::Quaternion(const vectorPt &v1, const vectorPt &v2) {
double angle=acos( v1.dot(v2)/(v1.norm()*v2.norm()) );
vectorPt axis=v1.cross(v2);
double norm=axis.norm();
if (norm<EPS) {
*this=NOROTATE;
return;
}
axis/=norm;
double halfa=angle/2;
double s=sin(halfa);
w=cos(halfa);
i=axis.x*s;
j=axis.y*s;
k=axis.z*s;
#ifdef NANCHECK
//assert(a::not_nan(w) && a::not_nan(i) && a::not_nan(j) && a::not_nan(k));
#endif
}
// From Euler angles
Quaternion::Quaternion(const EulerRot &erot) {
double c1 = cos(erot.theta/2);
double s1 = sin(erot.theta/2);
double c2 = cos(erot.psi/2);
double s2 = sin(erot.psi/2);
double c3 = cos(erot.phi/2);
double s3 = sin(erot.phi/2);
double c1c2 = c1*c2;
double s1s2 = s1*s2;
w =c1c2*c3 - s1s2*s3;
i =c1c2*s3 + s1s2*c3;
j =s1*c2*c3 + c1*s2*s3;
k =c1*s2*c3 - s1*c2*s3;
}
double & Quaternion::operator[](unsigned index) {
switch(index) {
case I: return i;
case J: return j;
case K: return k;
case W: return w;
}
return w;
}
Quaternion operator + (const Quaternion one, const Quaternion two) {
Quaternion q(one.w+two.w,one.i+two.i,one.j+two.j,one.k+two.k);
return q;
}
void Quaternion::operator += (const Quaternion &two) {
w+=two.w; i+=two.i; j+=two.j; k+=two.k;
}
Quaternion Quaternion::operator* (const Quaternion &two) const {
Quaternion q;
q.w=two.w*w-two.i*i-two.j*j-two.k*k;
q.i=two.w*i+two.i*w+two.j*k-two.k*j;
q.j=two.w*j-two.i*k+two.j*w+two.k*i;
q.k=two.w*k+two.i*j-two.j*i+two.k*w;
return (q);
}
void Quaternion::operator*=(const Quaternion &two) {
(*this)=(*this)*two;
}
Quaternion Quaternion::operator* (const double two) const {
Quaternion q;
q.w=two*w;
q.i=two*i;
q.j=two*j;
q.k=two*k;
return (q);
}
void Quaternion::operator*=(const double two) {
w*=two;
i*=two;
j*=two;
k*=two;
}
void Quaternion::operator = (const Quaternion &two) {
#ifdef NANCHECK
//assert(a::not_nan(two.w) && a::not_nan(two.i) && a::not_nan(two.j) &&
// a::not_nan(two.k));
#endif
w=two.w; i=two.i; j=two.j; k=two.k; return;
}
bool Quaternion::operator == (const Quaternion &two) const {
if (w!=two.w || i!=two.i || j!=two.j || k!=two.k)
return false;
return true;
}
Quaternion Quaternion::conj() const {
Quaternion q;
q.w=w; q.i=i*-1.0; q.j=j*-1.0; q.k=k*-1.0;
return q;
}
double Quaternion::norm() {
return (sqrt(w*w+i*i+j*j+k*k));
}
void Quaternion::normalize() {
double temp=norm();
if (temp!=0) {
w/=temp; i/=temp; j/=temp; k/=temp;
} else {
w=1; i=0; j=0; k=0;
}
}
Quaternion Quaternion::unit() {
Quaternion unit=*this;
unit.normalize();
return unit;
}
ostream & operator<<(ostream &out, const Quaternion &q) {
out << q.w << " " << q.i << " " << q.j << " " << q.k << " ";
return out;
}
istream & operator>>(istream &in, Quaternion &q) {
in >> q.w >> q.i >> q.j >> q.k;
return in;
}
// ------------------- RotMat Stuff
RotMat::RotMat() {
}
RotMat::RotMat(const Quaternion &q) {
set(q);
}
void RotMat::set(const Quaternion &q) {
// x'=x(w^2+i^2-k^2-j^2)+y(2ij-2kw)+z(2jw+2ik)
// y'=x(2ij+2kw)+y(j^2-k^2+w^2-i^2)+z(2jk-2iw)
// z'=x(2ik-2jw)+y(2jk+2iw)+z(k^2-j^2-i^2+w^2)
double w2=q.w*q.w;
double i2=q.i*q.i;
double j2=q.j*q.j;
double k2=q.k*q.k;
double twoij=2.0*q.i*q.j;
double twoik=2.0*q.i*q.k;
double twojk=2.0*q.j*q.k;
double twoiw=2.0*q.i*q.w;
double twojw=2.0*q.j*q.w;
double twokw=2.0*q.k*q.w;
x_x=w2+i2-j2-k2;
x_y=twoij-twokw;
x_z=twojw+twoik;
y_x=twoij+twokw;
y_y=w2-i2+j2-k2;
y_z=twojk-twoiw;
z_x=twoik-twojw;
z_y=twojk+twoiw;
z_z=w2-i2-j2+k2;
}
cPt RotMat::operator*(const cPt &in) const {
cPt out;
out.x=x_x*in.x+x_y*in.y+x_z*in.z;
out.y=y_x*in.x+y_y*in.y+y_z*in.z;
out.z=z_x*in.x+z_y*in.y+z_z*in.z;
return out;
}
void RotMat::proper() {
double det=x_x*y_y*z_z-x_x*y_z*z_y-x_y*y_x*z_z+x_y*y_z*z_x+x_z*y_x*z_y-x_z*
x_y*z_x;
if (det<0) {
x_x*=-1.0;
x_y*=-1.0;
x_z*=-1.0;
y_x*=-1.0;
y_y*=-1.0;
y_z*=-1.0;
z_x*=-1.0;
z_y*=-1.0;
z_z*=-1.0;
}
}
EulerRot::EulerRot() {
}
EulerRot::EulerRot(double theta_i,double psi_i,double phi_i) {
theta=theta_i;
psi=psi_i;
phi=phi_i;
}
// Rotate the rotation axis by a given rotation matrix
void EulerRot::rotate_axis(const RotMat &rotmat) {
double c1=cos(theta/2.0);
double s1=sin(theta/2.0);
double c2=cos(psi/2.0);
double s2=sin(psi/2.0);
double c3=cos(phi/2.0);
double s3=sin(phi/2.0);
double c1c2=c1*c2;
double s1s2=s1*s2;
double w=c1c2*c3 - s1s2*s3;
vectorPt axis;
axis.x=c1c2*s3 + s1s2*c3;
axis.y=s1*c2*c3 + c1*s2*s3;
axis.z=c1*s2*c3 - s1*c2*s3;
double angle=2.0*acos(w);
double norm = axis.norm();
if (norm < 0.00000001)
return; // No rotation
axis/=norm;
axis=rotmat*axis;
double s=sin(angle);
double c=cos(angle);
double t=1.0-c;
if ((axis.x*axis.y*t + axis.z*s) > 0.998) { // north pole singularity detected
theta = 2.0*atan2(axis.x*sin(angle/2.0),cos(angle/2.0));
psi = PI/2.0;
phi = 0;
return;
}
if ((axis.x*axis.y*t + axis.z*s) < -0.998) { // south pole singularity detected
theta = -2.0*atan2(axis.x*sin(angle/2.0),cos(angle/2.0));
psi = -PI/2.0;
phi = 0;
return;
}
theta = atan2(axis.y*s-axis.x*axis.z*t, 1-(axis.y*axis.y+axis.z*axis.z)*t);
psi = asin(axis.x*axis.y*t+axis.z*s) ;
phi = atan2(axis.x*s-axis.y*axis.z*t, 1-(axis.x*axis.x+axis.z*axis.z)*t);
}
void EulerRot::operator= (const EulerRot &two) {
theta=two.theta;
psi=two.psi;
phi=two.phi;
}
ostream & operator<<(ostream &out, const EulerRot &rot) {
out << rot.theta << " " << rot.psi << " " << rot.phi;
return out;
}
// ------------------- Global functions
// Find the point on the line in the direction of v closest to an outside point
// The point v_point is known to lie on the line
cPt c::point_to_line(const vectorPt &v, const cPt &v_point, const cPt &point){
vectorPt u=point-v_point;
double uTv=u.dot(v);
double vTv=v.dot(v);
double qT=uTv/vTv;
return ( (v*qT)+v_point);
}
// Return the closest distance between two line segments input as end points
double c::closest_approach(const cPt &l1_1, const cPt &l1_2, const cPt &l2_1,
const cPt &l2_2) {
vectorPt u = l1_2 - l1_1;
vectorPt v = l2_2 - l2_1;
vectorPt w = l1_1-l2_1;
double a = u.dot(u);
double b = u.dot(v);
double c = v.dot(v);
double d = u.dot(w);
double e = v.dot(w);
double D = a*c - b*b;
double sc, sN, sD = D;
double tc, tN, tD = D;
// compute the closest points between the two lines
if (D < 0.00000000001) { // parallel lines
sN = 0.0; sD = 1.0; tN = e; tD = c;
} else { // get the closest points on the infinite lines
sN = (b*e - c*d);
tN = (a*e - b*d);
if (sN < 0.0) {
sN = 0.0; tN = e; tD = c;
} else if (sN > sD) {
sN = sD; tN = e + b; tD = c;
}
}
if (tN < 0.0) {
tN = 0.0;
if (-d < 0.0)
sN = 0.0;
else if (-d > a)
sN = sD;
else {
sN = -d; sD = a;
}
} else if (tN > tD) {
tN = tD;
if ((-d + b) < 0.0)
sN = 0;
else if ((-d + b) > a)
sN = sD;
else {
sN = (-d + b); sD = a;
}
}
sc=sN/sD;
tc=tN/tD;
// vector connecting points
vectorPt dP=w+(u*sc)-(v*tc);
return dP.norm();
}
void c::closest_approach_points(const cPt &l1_1, const cPt &l1_2,
const cPt &l2_1, const cPt &l2_2,
cPt &close_l1, cPt &close_l2) {
vectorPt u = l1_2 - l1_1;
vectorPt v = l2_2 - l2_1;
vectorPt w = l1_1-l2_1;
double a = u.dot(u);
double b = u.dot(v);
double c = v.dot(v);
double d = u.dot(w);
double e = v.dot(w);
double D = a*c - b*b;
double sc, sN, sD = D;
double tc, tN, tD = D;
// compute the closest points between the two lines
if (D < 0.00000000001) { // parallel lines
sN = 0.0; sD = 1.0; tN = e; tD = c;
} else { // get the closest points on the infinite lines
sN = (b*e - c*d);
tN = (a*e - b*d);
if (sN < 0.0) {
sN = 0.0; tN = e; tD = c;
} else if (sN > sD) {
sN = sD; tN = e + b; tD = c;
}
}
if (tN < 0.0) {
tN = 0.0;
if (-d < 0.0)
sN = 0.0;
else if (-d > a)
sN = sD;
else {
sN = -d; sD = a;
}
} else if (tN > tD) {
tN = tD;
if ((-d + b) < 0.0)
sN = 0;
else if ((-d + b) > a)
sN = sD;
else {
sN = (-d + b); sD = a;
}
}
sc=sN/sD;
tc=tN/tD;
close_l1=u*sc+l1_1;
close_l2=v*tc+l2_1;
return;
}
// Returns true if two line segments intersect
bool c::intersect(const c2DPt &line1_start, const c2DPt &line1_end,
const c2DPt &line2_start, const c2DPt &line2_end) {
// Check segment1, line2 intersection
if (!sline_intersect(line1_start,line1_end,(line2_end-line2_start).normal(),
line2_start))
return false;
// Check segment2, line1 intersection
if (!sline_intersect(line2_start,line2_end,(line1_end-line1_start).normal(),
line1_start))
return false;
return true;
}
// Liang-Barsky Intersection between line segment and line
bool c::sline_intersect(const c2DPt &line1_start, const c2DPt &line1_end,
const c2DPt &line2normal, const c2DPt &line2_point) {
double denom=(line1_end-line1_start).dot(line2normal);
if (fabs(denom)<am::epsilon(denom))
return false; // Parallel lines
double numerator=(line1_start-line2_point).dot(line2normal)*-1.0;
double t=numerator/denom;
if (t<0 || t>1)
return false;
return true;
}
/// Average position
cPt c::mean(vector<cPt> &vec) {
cPt average(vec[0]);
for (unsigned i=1; i<vec.size(); i++)
average+=vec[i];
return average/vec.size();
}
template class TwoD<double>;
template class TwoD<float>;
template class TwoD<unsigned>;
template ostream & operator<< <double>(ostream &out, const TwoD<double> &t);
template ostream & operator<< <float>(ostream &out, const TwoD<float> &t);
template istream & operator>> <double>(istream &in, TwoD<double> &t);
template istream & operator>> <float>(istream &in, TwoD<float> &t);
template class ThreeD<int>;
template class ThreeD<float>;
template class ThreeD<double>;
template class ThreeD<unsigned>;
template ostream & operator<< <float>(ostream &out, const ThreeD<float> &t);
template istream & operator>> <float>(istream &in, ThreeD<float> &t);
template ostream & operator<< <double>(ostream &out, const ThreeD<double> &t);
template istream & operator>> <double>(istream &in, ThreeD<double> &t);
template ostream & operator<< <unsigned>(ostream &out, const ThreeD<unsigned> &t);
template istream & operator>> <unsigned>(istream &in, ThreeD<unsigned> &t);
template ThreeD<unsigned> operator+ (const unsigned one,
const ThreeD<unsigned> &two);
template ThreeD<double> operator+(const double one, const ThreeD<double> &two);
template ThreeD<unsigned> operator- (const unsigned one,
const ThreeD<unsigned> &two);
template ThreeD<double> operator-(const double one, const ThreeD<double> &two);
template ThreeD<unsigned> operator* (const unsigned one,
const ThreeD<unsigned> &two);
template ThreeD<double> operator*(const double one, const ThreeD<double> &two);
template ThreeD<unsigned> operator/ (const unsigned one,
const ThreeD<unsigned> &two);
template ThreeD<double> operator/(const double one, const ThreeD<double> &two);
Reference in New Issue View Git Blame Copy Permalink