//package Applet3D;

public class cQuat implements java.io.Serializable
{
    static final long serialVersionUID = -4375060109944420639L;
    
    public cQuat(double d, double d1, double d2, double d3)
    {
        double d4 = 1.0D / Math.sqrt(d * d + d1 * d1 + d2 * d2 + d3 * d3);
        x = d * d4;
        y = d1 * d4;
        z = d2 * d4;
        w = d3 * d4;
    }

    public cQuat(double ad[])
    {
        double d = 1.0D / Math.sqrt(ad[0] * ad[0] + ad[1] * ad[1] + ad[2] * ad[2] + ad[3] * ad[3]);
        x = ad[0] * d;
        y = ad[1] * d;
        z = ad[2] * d;
        w = ad[3] * d;
    }

    public cQuat(cQuat quat)
    {
        x = quat.x;
        y = quat.y;
        z = quat.z;
        w = quat.w;
    }

    public cQuat()
    {
    }

    public cQuat(cVector axe, double angle)
    {
	    set(axe, angle);
    }
    
    public cQuat(cVector pA, cVector pB)
    {
        setRotation(pA, pB);
    }
    
    public void setRotation(cVector pA, cVector pB)
    {
        if (pA == null || pB == null)
        {
            x = y = z = 0.0D;
            w = 1.0D;
            return;
        }
        cVector lA = new cVector(pA);
        cVector lB = new cVector(pB);
        if (lA.length() > 0)
            lA.normalize();
        if (lB.length() > 0)
            lB.normalize();
        double lDot = lA.dot(lB);
        cVector lCP = new cVector();
        lCP.cross(lA, lB);
        if (lCP.length() > 0)
            lCP.normalize();
        else
        if (lDot < 0.0D)
        {
            lCP.cross(lA, cVector.Y);
            if (lCP.length() > 0)
            {
                lCP.normalize();
            } else
            {
                lCP.cross(lA, cVector.X);
                lCP.normalize();
            }
        }
        double lTmp = (lDot + 1.0D) / 2D;
        double lCos = Math.sqrt(lTmp);
        double lSin = Math.sqrt(1.0D - lTmp);
        x = (double)lCP.x * lSin;
        y = (double)lCP.y * lSin;
        z = (double)lCP.z * lSin;
        w = lCos;
    }
    
    public void set(cVector axe, double angle)
    {
	    double s = Math.sin(angle/2);
	    double c = Math.cos(angle/2);
	    
	    axe.normalize();
	    
	    x = axe.x * s;
	    y = axe.y * s;
	    z = axe.z * s;
	    w = c;
    }
    
    public void conjugate(cQuat quat)
    {
        x = -quat.x;
        y = -quat.y;
        z = -quat.z;
        w = quat.w;
    }

    public void conjugate()
    {
        x = -x;
        y = -y;
        z = -z;
    }

    public void mul(cQuat quat, cQuat quat1)
    {
        if (this != quat && this != quat1)
        {
            w = quat.w * quat1.w - quat.x * quat1.x - quat.y * quat1.y - quat.z * quat1.z;
            x = (quat.w * quat1.x + quat1.w * quat.x + quat.y * quat1.z) - quat.z * quat1.y;
            y = ((quat.w * quat1.y + quat1.w * quat.y) - quat.x * quat1.z) + quat.z * quat1.x;
            z = (quat.w * quat1.z + quat1.w * quat.z + quat.x * quat1.y) - quat.y * quat1.x;
        } else
        {
            double d2 = quat.w * quat1.w - quat.x * quat1.x - quat.y * quat1.y - quat.z * quat1.z;
            double d = (quat.w * quat1.x + quat1.w * quat.x + quat.y * quat1.z) - quat.z * quat1.y;
            double d1 = ((quat.w * quat1.y + quat1.w * quat.y) - quat.x * quat1.z) + quat.z * quat1.x;
            z = (quat.w * quat1.z + quat1.w * quat.z + quat.x * quat1.y) - quat.y * quat1.x;
            w = d2;
            x = d;
            y = d1;
        }
    }

    public void mul(cQuat quat)
    {
        double d2 = w * quat.w - x * quat.x - y * quat.y - z * quat.z;
        double d = (w * quat.x + quat.w * x + y * quat.z) - z * quat.y;
        double d1 = ((w * quat.y + quat.w * y) - x * quat.z) + z * quat.x;
        z = (w * quat.z + quat.w * z + x * quat.y) - y * quat.x;
        w = d2;
        x = d;
        y = d1;
    }

    public void rotate(cVector p)
    {
	    double m00 = w*w + x*x - y*y - z*z;
	    double m01 = 2*(x*y - w*z);
	    double m02 = 2*(w*y + x*z);
	    double m10 = 2*(w*z + x*y);
	    double m11 = w*w - x*x + y*y - z*z;
	    double m12 = 2*(y*z - w*x);
	    double m20 = 2*(x*z - w*y);
	    double m21 = 2*(w*x + y*z);
	    double m22 = w*w - x*x - y*y + z*z;
	    
	    double fx = p.x*m00 + p.y*m01 + p.z*m02;
	    double fy = p.x*m10 + p.y*m11 + p.z*m12;
	    double fz = p.x*m20 + p.y*m21 + p.z*m22;
	    
	    /*
	    double tx = p.x*x;
	    double ty = p.y*x;
	    double tz = p.z*x;
	    
	    double fx = x*tx + y*ty + z*tz;

	    tx = p.x*y;
	    ty = p.y*y;
	    tz = p.z*y;
	    double fy = x*tx + y*ty + z*tz;

	    tx = p.x*z;
	    ty = p.y*z;
	    tz = p.z*z;
	    double fz = x*tx + y*ty + z*tz;
	    */
	    
	    p.x = (double)fx;
	    p.y = (double)fy;
	    p.z = (double)fz;
    }
    
    public void mulInverse(cQuat quat, cQuat quat1)
    {
        cQuat quat2 = new cQuat(quat1);
        quat2.inverse();
        mul(quat, quat2);
    }

    public void mulInverse(cQuat quat)
    {
        cQuat quat1 = new cQuat(quat);
        quat1.inverse();
        mul(quat1);
    }

    public void inverse(cQuat quat)
    {
        double d = 1.0D / (quat.w * quat.w + quat.x * quat.x + quat.y * quat.y + quat.z * quat.z);
        w = d * quat.w;
        x = -d * quat.x;
        y = -d * quat.y;
        z = -d * quat.z;
    }

    public void inverse()
    {
        double d = 1.0D / (w * w + x * x + y * y + z * z);
        w *= d;
        x *= -d;
        y *= -d;
        z *= -d;
    }

    public void normalize(cQuat quat)
    {
        double d = quat.x * quat.x + quat.y * quat.y + quat.z * quat.z + quat.w * quat.w;
        if (d > 0.0D)
        {
            d = 1.0D / Math.sqrt(d);
            x = d * quat.x;
            y = d * quat.y;
            z = d * quat.z;
            w = d * quat.w;
        } else
        {
            x = 0.0D;
            y = 0.0D;
            z = 0.0D;
            w = 0.0D;
        }
    }

    public void normalize()
    {
        double d = x * x + y * y + z * z + w * w;
        if (d > 0.0D)
        {
            d = 1.0D / Math.sqrt(d);
            x *= d;
            y *= d;
            z *= d;
            w *= d;
        } else
        {
            x = 0.0D;
            y = 0.0D;
            z = 0.0D;
            w = 0.0D;
        }
    }

    /*
    public final void set(Matrix4f matrix4f)
    {
        double d = 0.25D * (double)(matrix4f.m00 + matrix4f.m11 + matrix4f.m22 + matrix4f.m33);
        if (d >= 0.0D)
        {
            if (d >= 1.0000000000000001E-30D)
            {
                w = Math.sqrt(d);
                d = 0.25D / w;
                x = (double)(matrix4f.m21 - matrix4f.m12) * d;
                y = (double)(matrix4f.m02 - matrix4f.m20) * d;
                z = (double)(matrix4f.m10 - matrix4f.m01) * d;
                return;
            }
        } else
        {
            w = 0.0D;
            x = 0.0D;
            y = 0.0D;
            z = 1.0D;
            return;
        }
        w = 0.0D;
        d = -0.5D * (double)(matrix4f.m11 + matrix4f.m22);
        if (d >= 0.0D)
        {
            if (d >= 1.0000000000000001E-30D)
            {
                x = Math.sqrt(d);
                d = 1.0D / (2D * x);
                y = (double)matrix4f.m10 * d;
                z = (double)matrix4f.m20 * d;
                return;
            }
        } else
        {
            x = 0.0D;
            y = 0.0D;
            z = 1.0D;
            return;
        }
        x = 0.0D;
        d = 0.5D * (1.0D - (double)matrix4f.m22);
        if (d >= 1.0000000000000001E-30D)
        {
            y = Math.sqrt(d);
            z = (double)matrix4f.m21 / (2D * y);
            return;
        } else
        {
            y = 0.0D;
            z = 1.0D;
            return;
        }
    }

    public final void set(Matrix4d matrix4d)
    {
        double d = 0.25D * (matrix4d.m00 + matrix4d.m11 + matrix4d.m22 + matrix4d.m33);
        if (d >= 0.0D)
        {
            if (d >= 1.0000000000000001E-30D)
            {
                w = Math.sqrt(d);
                d = 0.25D / w;
                x = (matrix4d.m21 - matrix4d.m12) * d;
                y = (matrix4d.m02 - matrix4d.m20) * d;
                z = (matrix4d.m10 - matrix4d.m01) * d;
                return;
            }
        } else
        {
            w = 0.0D;
            x = 0.0D;
            y = 0.0D;
            z = 1.0D;
            return;
        }
        w = 0.0D;
        d = -0.5D * (matrix4d.m11 + matrix4d.m22);
        if (d >= 0.0D)
        {
            if (d >= 1.0000000000000001E-30D)
            {
                x = Math.sqrt(d);
                d = 0.5D / x;
                y = matrix4d.m10 * d;
                z = matrix4d.m20 * d;
                return;
            }
        } else
        {
            x = 0.0D;
            y = 0.0D;
            z = 1.0D;
            return;
        }
        x = 0.0D;
        d = 0.5D * (1.0D - matrix4d.m22);
        if (d >= 1.0000000000000001E-30D)
        {
            y = Math.sqrt(d);
            z = matrix4d.m21 / (2D * y);
            return;
        } else
        {
            y = 0.0D;
            z = 1.0D;
            return;
        }
    }

    public final void set(Matrix3f matrix3f)
    {
        double d = 0.25D * ((double)(matrix3f.m00 + matrix3f.m11 + matrix3f.m22) + 1.0D);
        if (d >= 0.0D)
        {
            if (d >= 1.0000000000000001E-30D)
            {
                w = Math.sqrt(d);
                d = 0.25D / w;
                x = (double)(matrix3f.m21 - matrix3f.m12) * d;
                y = (double)(matrix3f.m02 - matrix3f.m20) * d;
                z = (double)(matrix3f.m10 - matrix3f.m01) * d;
                return;
            }
        } else
        {
            w = 0.0D;
            x = 0.0D;
            y = 0.0D;
            z = 1.0D;
            return;
        }
        w = 0.0D;
        d = -0.5D * (double)(matrix3f.m11 + matrix3f.m22);
        if (d >= 0.0D)
        {
            if (d >= 1.0000000000000001E-30D)
            {
                x = Math.sqrt(d);
                d = 0.5D / x;
                y = (double)matrix3f.m10 * d;
                z = (double)matrix3f.m20 * d;
                return;
            }
        } else
        {
            x = 0.0D;
            y = 0.0D;
            z = 1.0D;
            return;
        }
        x = 0.0D;
        d = 0.5D * (1.0D - (double)matrix3f.m22);
        if (d >= 1.0000000000000001E-30D)
        {
            y = Math.sqrt(d);
            z = (double)matrix3f.m21 / (2D * y);
        }
        y = 0.0D;
        z = 1.0D;
    }

    public final void set(Matrix3d matrix3d)
    {
        double d = 0.25D * (matrix3d.m00 + matrix3d.m11 + matrix3d.m22 + 1.0D);
        if (d >= 0.0D)
        {
            if (d >= 1.0000000000000001E-30D)
            {
                w = Math.sqrt(d);
                d = 0.25D / w;
                x = (matrix3d.m21 - matrix3d.m12) * d;
                y = (matrix3d.m02 - matrix3d.m20) * d;
                z = (matrix3d.m10 - matrix3d.m01) * d;
                return;
            }
        } else
        {
            w = 0.0D;
            x = 0.0D;
            y = 0.0D;
            z = 1.0D;
            return;
        }
        w = 0.0D;
        d = -0.5D * (matrix3d.m11 + matrix3d.m22);
        if (d >= 0.0D)
        {
            if (d >= 1.0000000000000001E-30D)
            {
                x = Math.sqrt(d);
                d = 0.5D / x;
                y = matrix3d.m10 * d;
                z = matrix3d.m20 * d;
                return;
            }
        } else
        {
            x = 0.0D;
            y = 0.0D;
            z = 1.0D;
            return;
        }
        x = 0.0D;
        d = 0.5D * (1.0D - matrix3d.m22);
        if (d >= 1.0000000000000001E-30D)
        {
            y = Math.sqrt(d);
            z = matrix3d.m21 / (2D * y);
            return;
        } else
        {
            y = 0.0D;
            z = 1.0D;
            return;
        }
    }

    public final void set(AxisAngle4f axisangle4f)
    {
        double d1 = Math.sqrt(axisangle4f.x * axisangle4f.x + axisangle4f.y * axisangle4f.y + axisangle4f.z * axisangle4f.z);
        if (d1 < 9.9999999999999995E-07D)
        {
            w = 0.0D;
            x = 0.0D;
            y = 0.0D;
            z = 0.0D;
        } else
        {
            double d = Math.sin((double)axisangle4f.angle / 2D);
            d1 = 1.0D / d1;
            w = Math.cos((double)axisangle4f.angle / 2D);
            x = (double)axisangle4f.x * d1 * d;
            y = (double)axisangle4f.y * d1 * d;
            z = (double)axisangle4f.z * d1 * d;
        }
    }

    public final void set(AxisAngle4d axisangle4d)
    {
        double d1 = Math.sqrt(axisangle4d.x * axisangle4d.x + axisangle4d.y * axisangle4d.y + axisangle4d.z * axisangle4d.z);
        if (d1 < 9.9999999999999995E-07D)
        {
            w = 0.0D;
            x = 0.0D;
            y = 0.0D;
            z = 0.0D;
        } else
        {
            d1 = 1.0D / d1;
            double d = Math.sin(axisangle4d.angle / 2D);
            w = Math.cos(axisangle4d.angle / 2D);
            x = axisangle4d.x * d1 * d;
            y = axisangle4d.y * d1 * d;
            z = axisangle4d.z * d1 * d;
        }
    }
    */
    
    public void interpolate(cQuat quat, double d)
    {
        double d1 = x * quat.x + y * quat.y + z * quat.z + w * quat.w;
        if (d1 < 0.0D)
        {
            quat.x = -quat.x;
            quat.y = -quat.y;
            quat.z = -quat.z;
            quat.w = -quat.w;
        }
        double d2;
        double d3;
        if (1.0D - Math.abs(d1) > 9.9999999999999995E-07D)
        {
            double d4 = Math.acos(d1);
            double d5 = Math.sin(d4);
            d2 = Math.sin((1.0D - d) * d4) / d5;
            d3 = Math.sin(d * d4) / d5;
        } else
        {
            d2 = 1.0D - d;
            d3 = d;
        }
        w = d2 * w + d3 * quat.w;
        x = d2 * x + d3 * quat.x;
        y = d2 * y + d3 * quat.y;
        z = d2 * z + d3 * quat.z;
    }

    public void interpolate(cQuat quat, cQuat quat1, double d)
    {
        double d1 = quat1.x * quat.x + quat1.y * quat.y + quat1.z * quat.z + quat1.w * quat.w;
        if (d1 < 0.0D)
        {
            quat.x = -quat.x;
            quat.y = -quat.y;
            quat.z = -quat.z;
            quat.w = -quat.w;
        }
        double d2;
        double d3;
        if (1.0D - Math.abs(d1) > 9.9999999999999995E-07D)
        {
            double d4 = Math.acos(d1);
            double d5 = Math.sin(d4);
            d2 = Math.sin((1.0D - d) * d4) / d5;
            d3 = Math.sin(d * d4) / d5;
        } else
        {
            d2 = 1.0D - d;
            d3 = d;
        }
        w = d2 * quat.w + d3 * quat1.w;
        x = d2 * quat.x + d3 * quat1.x;
        y = d2 * quat.y + d3 * quat1.y;
        z = d2 * quat.z + d3 * quat1.z;
    }

    //static final long serialVersionUID = 0x69289dcfL;
    //static final double EPS = 9.9999999999999995E-07D;
    //static final double EPS2 = 1.0000000000000001E-30D;
    //static final double PIO2 = 1.57079632679D;
    
    double x,y,z,w;
}