// Trasform.cc
//	Originally created by Sugato Basu (1999??)
//	Modified by Kevin Karplus 

#include "Transform.h"
#include "FixStartLength.h"

static const Quaternion no_rot(1,0,0,0);	// identity quaternion
Transform Transform::identity (no_rot, XYZpoint::ZeroPoint);

// make sure that B has room for requested (Transformed) copy from A
// If number is negative, treat as stopping at a.NumPoints-number
// Return one past last position of A to copy.

static  long int MakeRoomForCopy(
	const Pointlist &A, Pointlist &B, long int startA,
	long int startB, long int& number)
{   
    assert(startA >=0 && startB >=0);
    assert(startA <= A.num_points());
    int past_end;

    if (number < 0)
    {   past_end = A.num_points()+number+1;
        assert(past_end>=startA);
	number = past_end-startA;
    }
    else    past_end = startA + number;

    // Make sure B is long enough
    if (B.num_points() < startB+number)
    {    B.append_point(0,0,0, startB+number-B.num_points());
    }
    
    return past_end;
}

// makes sure that A has enough room for inplace copy
// If number is negative, treat as stopping at a.NumPoints-number
// Return one past last position to copy.

static  long int MakeChecks( 
	const Pointlist &A, long int startA, long int& number)
{   
    assert(startA >=0);
    assert(startA <= A.num_points());
    int past_end;

    if (number < 0)
    {   past_end = A.num_points()+number+1;
        assert(past_end>=startA);
	number = past_end-startA;
    }
    else    past_end = startA + number;

    assert(A.num_points() >= past_end);

    return past_end;
}



inline static void Quaternion_to_matrix(const Quaternion& Q, double matrix[3][3])
{

	matrix[0][0] = Q.r()*Q.r() + Q.i()*Q.i() - Q.j()*Q.j() - Q.k()*Q.k();
	matrix[1][1] = Q.r()*Q.r() - Q.i()*Q.i() + Q.j()*Q.j() - Q.k()*Q.k();
	matrix[2][2] = Q.r()*Q.r() - Q.i()*Q.i() - Q.j()*Q.j() + Q.k()*Q.k();

	matrix[0][1] = 2*(Q.i()*Q.j() - Q.r()*Q.k());
	matrix[1][0] = 2*(Q.i()*Q.j() + Q.r()*Q.k());

	matrix[0][2] = 2*(Q.i()*Q.k() + Q.r()*Q.j());
	matrix[2][0] = 2*(Q.i()*Q.k() - Q.r()*Q.j());

	matrix[1][2] = 2*(Q.j()*Q.k() - Q.r()*Q.i());
	matrix[2][1] = 2*(Q.j()*Q.k() + Q.r()*Q.i());

}

void Transform::get_rotation_matrix(double matrix[3][3]) const
{
    Quaternion_to_matrix(Rotation, matrix);
}


///////////////////////////////////////////////////////////////////////
//	Different actions according to different sizes of Pointlist
//	---------------------------------------------------------- 
// 	0       return identity Transform
//	1       return pure translation
//      2 	(or purely colinear points)
//		return arbitrary correct Transformation
//      3       return unique solution using efficient computation
//		(but only if weights=0)
//      >=4     return unique solution using eigenvalue computation
///////////////////////////////////////////////////////////////////////

Transform::Transform(const Pointlist &A, 
	const Pointlist &B, 
	const float *weight,
	int start_a, int length, int start_b)
{   fix_start_length(start_a, length, A.num_points());
    fix_start_length(start_b, length, B.num_points());
    switch(length)
    { case 0:		// null transform
	(*this) = identity;
	return;
      case 1:		// translation only
	(*this) = identity;
	Offset = B[start_b] - A[start_a];
	return;

      case 3:
      	if (weight==NULL && 
		A[start_a] != A[start_a+1] &&
		A[start_a] != A[start_a+2] &&
		A[start_a+2] != A[start_a+1] &&
		B[start_b] != B[start_b+1] &&
		B[start_b] != B[start_b+2] &&
		B[start_b+2] != B[start_b+1]
	   )
        {   coplanar_trans(A,B, start_a, start_b);
	    return;
	}
	// fall through to default if weights used:
      default:
	optimal_transform(A,B,weight, start_a, length, start_b);
	return;
    }    
}

Transform::Transform(const Pointlist &A, 
	const Pointlist &B, 
	const bool *present,
	int start_a, int length, int start_b)
{   fix_start_length(start_a, length, A.num_points());
    fix_start_length(start_b, length, B.num_points());
    assert(length>=0);
    switch(length)
    { case 0:		// null transform
	(*this) = identity;
	return;
      case 1:		// translation only
	(*this) = identity;
	Offset = B[start_b] - A[start_a];
	return;
      case 3:
      	if (present==NULL && 
		A[start_a] != A[start_a+1] &&
		A[start_a] != A[start_a+2] &&
		A[start_a+2] != A[start_a+1] &&
		B[start_b] != B[start_b+1] &&
		B[start_b] != B[start_b+2] &&
		B[start_b+2] != B[start_b+1]
	   )
        {   coplanar_trans(A,B, start_a, start_b);
	    return;
	}
	// fall through to default if weights used:
      default:
	if (present != NULL)
	{   float *weight= new float[length];
	    for (int i=0; i<length; i++)
	    {	weight[i] = present[i]? 1.0: 0.0;
	    }
    	    optimal_transform(A,B,weight, start_a, length, start_b);
	    delete [] weight;
	}
	else 
	{    optimal_transform(A,B,NULL, start_a, length, start_b);
	}
	return;
    }    
}


// Construct a transformation that is a rotation of theta radians 
//	about the axis through points B and C.
// That is, if you have non-colinear points A,B,C,D,
//	applying the transformation to B or C leaves them unchanged,
//  applying the transformation to D increases the torsion angle(A,B,C,D)
//		by theta, but leaves distance(C,D) and bond angle(B,C,D) unchanged
Transform::Transform(const XYZpoint&B,  const XYZpoint&C, double theta)
{
    assert(B!=C);
    XYZpoint axis=C;
    axis-=B;
    axis.unitize();
    axis *= sin(0.5*theta);
    Rotation.r() = cos(0.5*theta);
    Rotation.i() = axis.X;
    Rotation.j() = axis.Y;
    Rotation.k() = axis.Z;
    
    // Make sure that B is a fixed point of the transformation
    XYZpoint Bimage;
    rotate(B, Bimage);
    
    Offset = B;
    Offset -= Bimage;
}


// convert a rotation matrix (such as is used by rotate_by_matrix)
// into a transform (pure rotation, no translation)
// matrix transforms point (1,0,0) into matrix[0][0], matrix[1][0], matrix[2][0]
Transform::Transform(const double matrix[3][3])
{
    // check for identity matrix
    for (int i=0; i<3; i++)
    {   for (int j=0; j<3; j++)
    	{    if (matrix[i][j] != (i==j? 1.0: 0.0))	goto NOTID;
	}
    }

    // is identity rotation:
    (*this)=Transform::identity;
    return;

    NOTID:
    
    // Since we are not guaranteed that the matrix is a real rotation matrix,
    // we'll do optimum superposition of a number of points.
    // This is undoubtedly not the most efficient way to do this conversion,
    // but it should be fairly robust, though it may have problems if
    // given a singular matrix, particularly an all-zero matrix.
    
    // The points are symmetrically chosen around the origin, to make
    // sure that the resulting optimal transform is a pure rotation.
    // (We map origin->origin and +-1 on each axis.)
    
    
    
    Pointlist from(7), to(7);
    from.append_point(XYZpoint::ZeroPoint);
    to.append_point(XYZpoint::ZeroPoint);

    XYZpoint x(1,0,0), rotx;
    rotate_by_matrix(matrix,x,rotx);
    from.append_point(x);
    to.append_point(rotx);
    x.X= -1;
    rotate_by_matrix(matrix,x,rotx);
    from.append_point(x);
    to.append_point(rotx);

    XYZpoint y(1,0,0), roty;
    rotate_by_matrix(matrix,y,roty);
    from.append_point(y);
    to.append_point(roty);
    y.Y= -1;
    rotate_by_matrix(matrix,y,roty);
    from.append_point(y);
    to.append_point(roty);

    XYZpoint z(1,0,0), rotz;
    rotate_by_matrix(matrix,z,rotz);
    from.append_point(z);
    to.append_point(rotz);
    z.Z= -1;
    rotate_by_matrix(matrix,z,rotz);
    from.append_point(z);
    to.append_point(rotz);
    
    optimal_transform(from, to);
    
    // Force the transform to be a pure rotation, even if
    // rounding errors moved the translation a bit away from (0,0,0)
    Offset = XYZpoint::ZeroPoint;
}


// On Bray (A Dell Dimension 4100 running Linux),
// optimal_transform (without weights)
// appears to take about 13.+0.28*num_points microseconds.

// On beta (an old DEC Alpha 4100 5/466) using g++
// optimal_transform (without weights)
// appears to take about 38 + 0.53 *num_points microseconds.
// (with CXX only 34+0.3*num_points microseconds)

// On sundance (an ultrasparc)
// optimal_transform (without weights)
// appears to take about 38 + 0.65 *num_points microseconds.


void Transform::optimal_transform(const Pointlist &A, 
	const Pointlist &B, 
	const float * weight,
	int start_a, int length, int start_b)
{   fix_start_length(start_a, length, A.num_points());
    fix_start_length(start_b, length, B.num_points());
    
    if (length==0)
    {   (*this)=Transform::identity;
	return;
    }
    
    XYZpoint Bcentroid=  B.compute_centroid(start_b, length,
    		weight? weight+start_a-start_b: NULL);
    if (! Bcentroid.is_finite()) 
    {   (*this)=identity; 
	return;
    }
    XYZpoint Acentroid=  A.compute_centroid(start_a, length,weight);
    if (! Acentroid.is_finite()) 
    {   (*this)=identity; 
	return;
    }
    Offset = Bcentroid-Acentroid;
    
    if (length==1)
    {  	Rotation = identity.Rotation;
	return;
    }
    else if (length==2)
    {  	Rotation = identity.Rotation;
        // determine rotation
	XYZpoint A0=A[start_a]-Acentroid;
	XYZpoint B0=B[start_b]-Bcentroid;
	if (A0==B0) return;	// no rotation needed
	double Amag=A0.magnitude();
	double Bmag=B0.magnitude();
	if (Amag==0 || Bmag==0) return;	// zero-length segment, no rotation
	// unitize the vectors:	
	A0 /= Amag;
	B0 /= Bmag;
	double costheta = dot(A0,B0);
	if (costheta > 0.99999) return;	// no rotation needed
	double theta=acos(costheta);
	
	XYZpoint axis=A0.cross(B0);
	axis.unitize();
	axis *= sin(0.5*theta);
	Rotation.r() = cos(0.5*theta);
	Rotation.i() = axis.X;
	Rotation.j() = axis.Y;
	Rotation.k() = axis.Z;
        XYZpoint tmp;
   	rotate(Acentroid,tmp);
    	Offset = Bcentroid- tmp;

#ifdef DEBUG_TRANSFORM_2POINT	
	// DEBUGGING printout
	Pointlist Aimage(2);
	trans(A,Aimage);
	cerr << "DEBUG: 2-point transform maps " 
		<< A[0] << " to " << Aimage[0] << " desired="<<B[0]
		<< "\n"
		<< A[1] << " to " << Aimage[1] << " desired="<<B[1]
		<< "\n";
#endif        
	return;
    }
    
    Pointlist Acopy(A,start_a,length);
    Acopy -= Acentroid;
    Pointlist Bcopy(B,start_b,length);
    Bcopy -= Bcentroid;
    
    Rotation = Optimal_rotation(Acopy, Bcopy, weight? weight-start_a: NULL);
//  cerr << Rotation;

    XYZpoint tmp;
    rotate(Acentroid,tmp);
    Offset = Bcentroid- tmp;
}

void Transform::xyplane(
	const XYZpoint& orig, const XYZpoint& x, const XYZpoint& y)
{
    if (x == orig) 
    {    XYZpoint fake_x(1,0,0);
    	fake_x += x;
	return xyplane(orig, fake_x, y);
    }
    
    if (y==orig || y==x)
    {   XYZpoint fake_y(0,1,0);
    	fake_y += y;
	return xyplane(orig, x, fake_y);
    }

    
    
    Pointlist src_points(3);
    src_points.append_point(orig);
    src_points.append_point(x);
    src_points.append_point(y);

    double xo2=sq_dist(orig,x);
    double xy2=sq_dist(y,x);
    double yo2=sq_dist(y,orig);
    
    double xo=sqrt(xo2);
    
    // guess the destinations
    Pointlist dest_points(3);
    dest_points.append_point(0,0,0);
    dest_points.append_point(xo,0,0);
    
    assert(xo != 0);
    double ydest_x= (xo2+yo2-xy2) / (2*xo);
    double ydest_y= sqrt(yo2-ydest_x*ydest_x);
    dest_points.append_point(ydest_x, ydest_y, 0);
    coplanar_trans(src_points, dest_points);
    
    // get real destinations
    Pointlist check_dest(3);
    trans(src_points, check_dest);
    // make sure origin is fixed up to really go to 0
    offset() -= check_dest[0];

#ifdef DEBUG    
    // THIS PART IS ERROR CHECKING, AND SHOULD
    // BE ELIMINATED ONCE CODE IS TRUSTED.
    double orig_dist2 = dot(check_dest[0], check_dest[0]);
    if (orig_dist2 > 1.0e-6 
	
	|| check_dest[1].X < 0.
    	|| (check_dest[1].Y*check_dest[1].Y + 
		check_dest[1].Z*check_dest[1].Z) > 1.0e-6
	
	|| check_dest[2].Y < 0.
	|| check_dest[2].Z*check_dest[2].Z > 1.0e-6
	)
    {	cerr << "Transform of\n"
    		<< src_points[0] << "\t"
    		<< src_points[1] << "\t"
    		<< src_points[2] << "\n"
    		<< " not correctly on xy-plane as\n"
    		<< dest_points[0] << "\t"
    		<< dest_points[1] << "\t"
    		<< dest_points[2] << "\n"
		<< "Instead Transformed to \n"
    		<< check_dest[0] << "\t"
    		<< check_dest[1] << "\t"
    		<< check_dest[2] << "\n"
		;
    }
#endif
}


ostream &operator<<(ostream &stream, const Transform& T)
{
   stream << T.Rotation << " " << T.Offset;
  return stream;
}

  
istream &operator>>(istream &stream, Transform &T)
{
  stream >> T.Rotation;
  stream >> T.Offset;

  return stream;
}



void Transform::rotate(const Pointlist &A, Pointlist &B, long int startA,
		long int startB, long int number) const
{   int past_end = MakeRoomForCopy(A,B,startA,startB,number);

    double RotationMatrix[3][3];
    get_rotation_matrix(RotationMatrix);

    int j = startB;
    for(int i=startA; i<past_end; i++,j++)
    {	rotate_by_matrix(RotationMatrix,A[i],B[j]);
    	// rotate(A[i],B[j]);
    }
}       
	
void Transform::shift(const Pointlist &A, Pointlist &B, 
	long int startA, long int startB, long int number) const
{   int past_end = MakeRoomForCopy(A,B,startA,startB,number);

    int j = startB;
    for(int i=startA; i<past_end; i++,j++)
    {	shift(A[i],B[j]);
    }
}       

void Transform::trans(const Pointlist &A, Pointlist &B, long int startA,
		long int startB, long int number)  const

{   int past_end = MakeRoomForCopy(A,B,startA,startB,number);

    double RotationMatrix[3][3];
    get_rotation_matrix(RotationMatrix);

    int j = startB;
    for(int i=startA; i<past_end; i++,j++)
    {	rotate_by_matrix(RotationMatrix,A[i],B[j]);
    	// rotate(A[i],B[j]);
    	B[j]+= Offset;	
    }
}



void Transform::rotateIP(Pointlist &A, long int startA, long int
number) const

{
    int past_end = MakeChecks(A,startA,number);

    double RotationMatrix[3][3];
    get_rotation_matrix(RotationMatrix);

    XYZpoint temp;
    for(int i=startA; i< past_end; i++)
    {	rotate_by_matrix(RotationMatrix,A[i],temp);
	A[i] = temp;
    }
}

void Transform::shiftIP(Pointlist &A, long int startA,long int number)  const

{
        int past_end = MakeChecks(A,startA,number);

	XYZpoint temp;
        for(int i=startA; i<past_end; i++)
        {	
		shift(A[i],temp);
		A[i] = temp;
	}
}

void Transform::transIP(Pointlist &A, long int startA,long int number)const
{
    int past_end = MakeChecks(A,startA,number);

    double RotationMatrix[3][3];
    get_rotation_matrix(RotationMatrix);

    XYZpoint temp;
    for(int i=startA; i< past_end; i++)
    {	rotate_by_matrix(RotationMatrix,A[i],temp);
	A[i] = temp;
	A[i]+=Offset;
    }
}

void Transform::coplanar_trans(const Pointlist &A, const Pointlist &B,
	int start_a, int start_b ) 
{
        assert(A.num_points() >= start_a+3);
        assert(B.num_points() >= start_b+3);

        Offset =  B.compute_centroid(start_b,3);
        if (! Offset.is_finite()) 
	{   (*this)=identity; 
	    return;
	}
	
	XYZpoint Acentroid =  A.compute_centroid(start_a,3);
        if (! Acentroid.is_finite()) 
	{   (*this)=identity; 
	    return;
	}
    	
	Pointlist A_centered(A,start_a,3);
        A_centered -= Acentroid;
        Pointlist B_centered(B,start_b,3);
        B_centered -= Offset;

        XYZpoint NA;	// unit normal to A plane
	cross(NA, A_centered[0], A_centered[1]);
	NA.unitize();
        
	XYZpoint NB;	// unit normal to B plane
	cross(NB, B_centered[0], B_centered[1]);
	NB.unitize();
	
#ifdef DEBUG_XY
        cerr << "NA= " << NA << " NB= " << NB << endl << flush;
#endif
        double dotQ = dot(NA,NB);

        XYZpoint intersect;	// vector parallel to intersection of planes
        cross(intersect, NA, NB);
        double magIntersect = intersect.magnitude();
	
	Quaternion Rot1;
	if (magIntersect!=0) 
	{   // NA and NB cross, so get the rotation that moves  NA to NB
	    intersect = intersect / magIntersect;
	    double cosphi = dotQ;
	    double sinphi = magIntersect;

#ifdef DEBUG_XY
	    cerr << "intersect= " <<  intersect << endl << flush;

	    double one;
	    one = sinphi *sinphi + cosphi * cosphi;
	    cerr << "sinphi= " <<sinphi << " cosphi= " << cosphi 
		    << " sin^2+cos^2= " << one << endl << flush;
#endif        
	    double sinhalfphi = (cosphi<=-1+1.e-8)? 1: sinphi / sqrt (2*(1 + cosphi));
	    assert(isfinite(sinhalfphi));
	    double coshalfphi = (cosphi<=-1+1.e-8)? 0: sqrt ((1 + cosphi)/2);
	    assert(isfinite(coshalfphi));

	    Quaternion tmp(coshalfphi, 
		    intersect.X*sinhalfphi, intersect.Y*sinhalfphi, intersect.Z*sinhalfphi);
	    Rot1 = tmp;
	}
	else
	{   // NA and NB are parallel (or anti-parallel)
	    // which means that A_centered and B_centered are in the
	    // same plane, and we need either a simple rotation (Rot2)
	    // or a flip followed by a simple rotation.
	    if (dotQ>0)	
	    {    // simple rotation
	    	Rot1 = identity.rotation();
	    }
	    else
	    {   // flip by rotating about any vector perpendicular to NA	        
	    	Rot1.r()=0;
		if (NA.X!=0)
		{   Rot1.i() = NA.Y;
		    Rot1.j() = -NA.X;
		    Rot1.k() = 0;
		}
		else
		{   Rot1.i()=0;
		    Rot1.j() = -NA.Z;
		    Rot1.k() = NA.Y;
		}
	    }
	}

	Rot1.unitize();	// fix up if magnitude slightly off
        Rotation = Rot1;
        rotateIP(A_centered);

#ifdef DEBUG_XY
        cerr  << "Rotated into plane A_centered=" 
		<< A_centered[0] << " ; "  
		<< A_centered[1] << " ; "  
		<< A_centered[2] << " ; "  
	        << endl  << flush;
#endif
        double C=0;
	for (int i=0; i< 3; i++)
                C += B_centered[i]&A_centered[i];
                
        XYZpoint Sum(B_centered[0]*A_centered[0]);
	Sum +=  B_centered[1]*A_centered[1];
	Sum +=  B_centered[2]*A_centered[2];
        double S =  - (NB & Sum);

        double mag = sqrt(S*S + C*C);
	assert(mag>0);
	double sintheta = S/mag;
        double costheta = C/mag;

#ifdef DEBUG_XY
        cerr << "sintheta= " << sintheta 
		<< " costheta= " << costheta << endl<< flush;
#endif
        double sinhalftheta = costheta<=-1+1.e-08 ?  (sintheta>0? 1: -1): sintheta / sqrt (2*(1 + costheta));
        assert(isfinite(sinhalftheta));
	double coshalftheta = costheta<=-1+1.e-08 ? 0: sqrt ((1 + costheta)/2);
        assert(isfinite(coshalftheta));
                
#ifdef DEBUG_XY
        cerr << "sinhalftheta= " << sinhalftheta 
		<< " coshalftheta= " << coshalftheta << endl<< flush;
#endif
	Quaternion Rot2(coshalftheta, 
		NB.X *sinhalftheta, NB.Y*sinhalftheta, NB.Z*sinhalftheta);
	Rot2.unitize();	// fix up if magnitude slightly off

#ifdef DEBUG_XY
	cerr << " Rot1= " << Rot1 << " Rot2= " << Rot2 << "\n" << flush;
#endif
        XYZpoint tmp;
        Rotation = Rot2*Rot1;
       	Rotation.unitize();	// fix up if magnitude slightly off
	rotate(Acentroid,tmp);
        Offset  -= tmp;
}

// crude recursive implementation of Euclid's algorithm
static int gcd(int a, int b)
{
    if (a<0) return gcd(-a,b);
    if (b<0) return gcd(a,-b);
    if (b>a) return gcd(b,a);
    assert( a>=0 && b >=0 && a>=b);
    if (b==0) return a;
    return gcd(b, a%b);
}


//	normalize this to be an nth root of the identity transform
void Transform::normalize_nth_root(int nmer)
{
    // make this^nmer be identity

    const double TINY= 1.e-7;
    if (rotation().r() < -TINY
    	|| (rotation().r() < TINY 
	    && ( rotation().i() < -TINY 
	        || (rotation().i()<TINY 
		    && ( rotation().j() <-TINY
		        || (rotation().j() < TINY
			    && rotation().k() <0))))))
    {   // normalize rotation to have positive rotation by flipping axis
	rotation().r() *=-1;
	rotation().i() *=-1;
	rotation().j() *=-1;
	rotation().k() *=-1;
    }
    assert(rotation().r() >= 1.e-7 
     || (rotation().r() >= -1.e-7 
	 && (rotation().i() >= 1.e-7 
	     || (rotation().i() >= -1.e-7 
		 && (rotation().j() >= 1.e-7 
		     || (rotation().j() >= -1.e-7 
			 && rotation().k() >= 0))))));


    if (nmer==0) return;
    if (nmer==1) 
    {    *(this)=identity;
    	return;
    }
    assert(nmer >1);

    
    // The cosine of half the angle of rotation is real part of the quaternion.
    // We want half_angle = pi/nmer (or perhaps k*pi/nmer for integer k).
    double half_angle = acos(static_cast<double>(rotation().r()));
    assert(0<= half_angle && half_angle<=M_PI);
    double desired_half_angle = M_PI /nmer;

    double ratio = half_angle/desired_half_angle;
    int k = static_cast<int>(ratio+0.5);
    assert(0<=k && k <=nmer);
    
    if (gcd(nmer,k) != 1)
    {   // we don't really want a transform that is a lower root of unity
        k=1;
    }
    
#ifdef DEBUG_NTHROOT    
    if (abs(ratio-k) >0.01)
    {    cerr << "Warning: transform not close to cyclic " << nmer
	    << "-mer, half angle= " << half_angle << " is " << ratio
	    << " times desired " << desired_half_angle 
	    << "\n   Could be " << M_PI/half_angle << "-mer??\n"		
	    << "   Transform is "
	    << t << "\n" << flush;
    }
#endif

    double target_half_angle = k*desired_half_angle;
    double newr=cos(target_half_angle);
    double news=sqrt(1-newr*newr);

    XYZpoint axis=    rotation().axis_vector();
    if (axis == XYZpoint::ZeroPoint)
    {    // problem: can't rotate without an axis, so make one up
    	axis.Z = 1.0;
    }
    axis.unitize();

    rotation().r()=newr;
    rotation().i()=news*axis.X;
    rotation().j()=news*axis.Y;
    rotation().k()=news*axis.Z;

    rotation().unitize();

    double dot_prod = dot(offset(), axis);
#ifdef DEBUG_NTHROOT    
    double offset_mag= offset().magnitude();
    if (abs(dot_prod) > 0.01 * (offset_mag>1? offset_mag: 1))
    {    cerr << "Warning: translation not perpendicular to rotation in supposedly cyclic transformation\n"
	    << " dot-product of translation with unitized axis = " << dot_prod << "\n"
	    << " transform = " << t << "\n"
	    ;
    }
#endif
    
    offset() -= axis * dot_prod ;
    
    if (rotation().r() < -TINY
    	|| (rotation().r() < TINY 
	    && ( rotation().i() < -TINY 
	        || (rotation().i()<TINY 
		    && ( rotation().j() <-TINY
		        || (rotation().j() < TINY
			    && rotation().k() <0))))))
    {   // normalize rotation to have positive rotation by flipping axis
	rotation().r() *=-1;
	rotation().i() *=-1;
	rotation().j() *=-1;
	rotation().k() *=-1;
    }
    assert(rotation().r() >= 1.e-7 
     || (rotation().r() >= -1.e-7 
	 && (rotation().i() >= 1.e-7 
	     || (rotation().i() >= -1.e-7 
		 && (rotation().j() >= 1.e-7 
		     || (rotation().j() >= -1.e-7 
			 && rotation().k() >= 0))))));
}

// Change Log:
//	March-April 2002 Lots of changes by Kevin Karplus not itemized.
//	
//	16  April 2002 Kevin Karplus
//	    added sublist parameters to Transform constructor,
//	    optimal_transform, and coplanar_transform.
//	
// 8 May 2002 Kevin Karplus
//	Fixed bug with weight==NULL in subscripted optimal_transform.
//	Added FixStartLength.h
// 9 May 2002 Kevin Karplus
//	Changed ==-1 tests for cosphi and costheta to <=-1 to avoid
//		sqrt(negative number)
//   22 Jan 2004 Kevin Karplus
//	Added constructor for rotation about an axis.
// 24 Jan 2005 Kevin Karplus
//	Moved normalize_nth_root from undertaker's Multimer.cc class.
//	Added test in normalize_nth_root for missing axis of rotation.
// Mon Feb  7 09:16:53 PST 2005 Kevin Karplus
//	Added constructor with bool array marking points that are present. 
// 16 Feb 2004 Kevin Karplus
//	Fixed optimal_transform for 2-point transforms
// Sat Jun  4 16:31:09 PDT 2005 Kevin Karplus
//	Added extra test in 2-point transforms for costheta near 1.
// Sun May 14 10:04:51 PDT 2006 Kevin Karplus
//	Added gcd test in normalize_nth_root to avoid getting
//	identity transform and roots lower than nth.
// Fri May 19 17:34:45 PDT 2006 Kevin Karplus
//	Fixed normalize_nth_root, so that values very close to 0 for rotation
//		cause sign to be chosen on next dimension
// Fri May 19 18:33:51 PDT 2006 Kevin Karplus
//	Also allow nmer=0 or nmer=1 in normalize_nth_root
// Tue May 23 16:05:30 PDT 2006 Kevin Karplus
//	added small error tolerance to test of cosphi<=-1
// Tue May 23 16:50:00 PDT 2006 Kevin Karplus
//	added test for already coplanar points in coplanar_trans
// Tue Jun 13 09:11:52 PDT 2006 Kevin Karplus
//	Added conversion from rotation matrix to Transform
// Tue Jun 13 09:25:21 PDT 2006 Kevin Karplus
//	Added get_rotation_matrix
// Fri Sep  8 09:09:55 PDT 2006 Kevin Karplus
//	Added test for identity transformation in conversion from rotation matrix to Transform
// Wed Jul  2 15:34:43 PDT 2008 Kevin Karplus
//	Added tests in xyplane to avoid divide-by-zero problems.
//	Also added assertions for sinhalfphi, coshalfphi, sinhalftheta, coshalftheta
// Mon Apr 27 17:51:41 PDT 2009 Kevin Karplus
//	Added identity returns to coplanar_trans and optimal_trans,
//	when centroids not defined.