# $CVSHeader: pal8e/src/CoordCalc.py,v 1.4 2005/02/15 01:10:25 indraneel Rel $

import sys
import math
import Numeric
from Scientific.Geometry import VectorModule

def dist_sqr(a, b):
    '''
    Returns square of distance
    '''
    dist_sqr = Numeric.sum((a-b)*(a-b))
    return dist_sqr

def distance(a, b):
    '''
    distance between coordinates a and b
    '''
    dist = math.sqrt(dist_sqr(a, b))
    return dist

def torsion(a, b, c, d):
    '''
    Find normals to each plane:
      Do a cross product between vectors b->a and b->c
      Do a cross product between vectors b->c and d->c
    Find angle between the normals (torsion angle)
    Find cross product of normals to get a temporary vector
    Check if direction of temporary vector is same as b->c
      Find dot product between b->c and temporary vector
      Reverse sign of torsion angle if sign of angle is negative
    Convert angle to degrees
    '''
    v1 = VectorModule.Vector(a-b)
    v2 = VectorModule.Vector(c-b)
    v3 = VectorModule.Vector(c-d)
    n1 = v1.cross(v2)
    n2 = v3.cross(v2)
    torsion_angle = n1.angle(n2)
    tvec = n2.cross(n1)
    tdot = Numeric.dot(v2,tvec)
    if tdot < 0:
        torsion_angle *= -1
    if torsion_angle > 0:
        torsion_angle = math.pi - torsion_angle
    else:
        torsion_angle = -1 * ( math.pi + torsion_angle )
    torsion_angle = torsion_angle / math.pi * 180
    return torsion_angle

def v_angle(v1, v2, range=180):
    '''
    Find angle between vectors v1 and v2
    (Arguments v1 and v2 are converted to a vector object before calculation)
    '''
    v1 = VectorModule.Vector(v1)
    v2 = VectorModule.Vector(v2)
    angle = v1.angle(v2)
    if range == None:
        return angle
    angle = angle / math.pi * 180
    ## debug statements for testing code
    if angle < 0:
        print >> sys.stderr, "WARNING:: NEGATIVE ANGLE DETECTED"
    ## end of test statements
    if range == 90 and angle > 90:
        angle = 180 - angle
    return angle

