Index: /trunk/psLib/src/mathtypes/psVector.c
===================================================================
--- /trunk/psLib/src/mathtypes/psVector.c	(revision 6976)
+++ /trunk/psLib/src/mathtypes/psVector.c	(revision 6977)
@@ -9,6 +9,6 @@
 *  @author Robert DeSonia, MHPCC
 *
-*  @version $Revision: 1.67 $ $Name: not supported by cvs2svn $
-*  @date $Date: 2006-04-18 22:12:28 $
+*  @version $Revision: 1.68 $ $Name: not supported by cvs2svn $
+*  @date $Date: 2006-04-25 03:30:29 $
 *
 *  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
@@ -207,4 +207,37 @@
     output = psVectorRecycle(output, nElements, type);
     output->n = nElements;
+
+    if (input->type.type == type) {
+        // Can simply copy the bytes if the types are the same
+
+        #define PSVECTOR_COPY_SAME_CASE(NAME) \
+    case PS_TYPE_##NAME: \
+        output->data.NAME = memcpy(output->data.NAME, input->data.NAME, \
+                                   input->n * PSELEMTYPE_SIZEOF(PS_TYPE_##NAME)); \
+        break;
+
+        switch (type) {
+            PSVECTOR_COPY_SAME_CASE(U8);
+            PSVECTOR_COPY_SAME_CASE(U16);
+            PSVECTOR_COPY_SAME_CASE(U32);
+            PSVECTOR_COPY_SAME_CASE(U64);
+            PSVECTOR_COPY_SAME_CASE(S8);
+            PSVECTOR_COPY_SAME_CASE(S16);
+            PSVECTOR_COPY_SAME_CASE(S32);
+            PSVECTOR_COPY_SAME_CASE(S64);
+            PSVECTOR_COPY_SAME_CASE(F32);
+            PSVECTOR_COPY_SAME_CASE(F64);
+            PSVECTOR_COPY_SAME_CASE(C32);
+            PSVECTOR_COPY_SAME_CASE(C64);
+        default: {
+                char* typeStr;
+                PS_TYPE_NAME(typeStr,type);
+                psError(PS_ERR_BAD_PARAMETER_TYPE, true, PS_ERRORTEXT_psVector_UNSUPPORTED_TYPE, typeStr);
+                psFree(output);
+                return NULL;
+            }
+        }
+        return output;
+    }
 
     #define PSVECTOR_COPY(INTYPE,OUTTYPE) { \
@@ -296,13 +329,112 @@
 }
 
+// Heap sort of the array
+#define PSVECTOR_SORT_CASE(NAME) \
+case PS_TYPE_##NAME: { \
+    ps##NAME temp; \
+    ps##NAME *value = out->data.NAME; \
+    for (;;) { \
+        if (l > 0) { \
+            temp = value[--l]; \
+        } else { \
+            temp = value[ir]; \
+            value[ir] = value[0]; \
+            if (--ir == 0) { \
+                value[0] = temp; \
+                return out; \
+            } \
+        } \
+        int i = l; \
+        int j = (l << 1) + 1; \
+        while (j <= ir) { \
+            if (j < ir && value[j] < value[j+1]) \
+                ++j; \
+            if (temp < value[j]) { \
+                value[i]=value[j]; \
+                j += (i=j) + 1; \
+            } else { \
+                j = ir + 1; \
+            } \
+        } \
+        value[i] = temp; \
+    } \
+} \
+break;
+
 psVector* psVectorSort(psVector* outVector,
                        const psVector* inVector)
 {
-    psS32 N = 0;
-    psS32 elSize = 0;
-    psPtr inVec = NULL;
-    psPtr outVec = NULL;
-    psElemType inType = 0;
-
+    if (!inVector) {
+        psError(PS_ERR_BAD_PARAMETER_NULL, true,
+                PS_ERRORTEXT_psVector_SORT_NULL);
+        psFree(outVector);
+        return NULL;
+    }
+
+    psVector *out = psVectorCopy(outVector, inVector, inVector->type.type); // Copy to sort in place
+    long N = out->n;                    // Number of elements
+    if (N < 2) {
+        return out;
+    }
+    long l = N >> 1;
+    long ir = N - 1;
+    switch (out->type.type) {
+        PSVECTOR_SORT_CASE(U8);
+        PSVECTOR_SORT_CASE(U16);
+        PSVECTOR_SORT_CASE(U32);
+        PSVECTOR_SORT_CASE(U64);
+        PSVECTOR_SORT_CASE(S8);
+        PSVECTOR_SORT_CASE(S16);
+        PSVECTOR_SORT_CASE(S32);
+        PSVECTOR_SORT_CASE(S64);
+        PSVECTOR_SORT_CASE(F32);
+        PSVECTOR_SORT_CASE(F64);
+    default:
+        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
+                PS_ERRORTEXT_psVector_UNSUPPORTED_TYPE,
+                inVector->type.type);
+        psFree(outVector);
+        return NULL;
+    }
+    return out;
+}
+
+// Heap sort of the index array
+#define PSVECTOR_SORT_INDEX_CASE(NAME) \
+case PS_TYPE_##NAME: { \
+    psU32 temp; \
+    psU32 *index = out->data.U32; \
+    ps##NAME *value = inVector->data.NAME; \
+    for (;;) { \
+        if (l > 0) { \
+            temp = index[--l]; \
+        } else { \
+            temp = index[ir]; \
+            index[ir] = index[0]; \
+            if (--ir == 0) { \
+                index[0] = temp; \
+                return out; \
+            } \
+        } \
+        int i = l; \
+        int j = (l << 1) + 1; \
+        while (j <= ir) { \
+            if (j < ir && value[index[j]] < value[index[j+1]]) \
+                ++j; \
+            if (value[temp] < value[index[j]]) { \
+                index[i]=index[j]; \
+                j += (i=j) + 1; \
+            } else { \
+                j = ir + 1; \
+            } \
+        } \
+        index[i] = temp; \
+    } \
+} \
+break;
+
+psVector* psVectorSortIndex(psVector* outVector,
+                            const psVector* inVector)
+{
     if (inVector == NULL) {
         psError(PS_ERR_BAD_PARAMETER_NULL, true,
@@ -312,159 +444,31 @@
     }
 
-    inType = inVector->type.type;
-    N = inVector->n;
-    inVec = (psPtr)inVector->data.U8;
-    elSize = PSELEMTYPE_SIZEOF(inType);
-
-    if (outVector == NULL) {
-        outVector = psVectorAlloc(N, inType);
-    }
-
-    // check to see if output vector needs to be resized/retyped
-    if ( (N > outVector->nalloc) ||
-            (inType != outVector->type.type) ) {
-        // reshape the output vector to match the input vector's size/type.
-        outVector = psVectorRecycle(outVector,N,inType);
-    }
-    outVector->n = N;
-    outVec = outVector->data.U8;
-
-    if (N == 0) {
-        // no need to sort anything, as there are no elements in input vector.
-        return outVector;
-    }
-
-    // Copy input vector values into output vector if not in-place sorting
-    if (inVector != outVector) {
-        memcpy(outVec, inVec, elSize * N);
-    }
-
-    // Sort output vector
-    switch (inType) {
-    case PS_TYPE_U8:
-        qsort(outVec, N, elSize, psCompareU8);
-        break;
-    case PS_TYPE_U16:
-        qsort(outVec, N, elSize, psCompareU16);
-        break;
-    case PS_TYPE_U32:
-        qsort(outVec, N, elSize, psCompareU32);
-        break;
-    case PS_TYPE_U64:
-        qsort(outVec, N, elSize, psCompareU64);
-        break;
-    case PS_TYPE_S8:
-        qsort(outVec, N, elSize, psCompareS8);
-        break;
-    case PS_TYPE_S16:
-        qsort(outVec, N, elSize, psCompareS16);
-        break;
-    case PS_TYPE_S32:
-        qsort(outVec, N, elSize, psCompareS32);
-        break;
-    case PS_TYPE_S64:
-        qsort(outVec, N, elSize, psCompareS64);
-        break;
-    case PS_TYPE_F32:
-        qsort(outVec, N, elSize, psCompareF32);
-        break;
-    case PS_TYPE_F64:
-        qsort(outVec, N, elSize, psCompareF64);
-        break;
+    psVector *out = psVectorRecycle(outVector, inVector->n, PS_TYPE_U32); // Vector for output
+    out = psVectorCreate(out, 0, inVector->n, 1, PS_TYPE_U32);
+    long N = out->n;                    // Number of elements
+    if (N < 2) {
+        return out;
+    }
+    long l = N >> 1;
+    long ir = N - 1;
+    switch (out->type.type) {
+        PSVECTOR_SORT_INDEX_CASE(U8);
+        PSVECTOR_SORT_INDEX_CASE(U16);
+        PSVECTOR_SORT_INDEX_CASE(U32);
+        PSVECTOR_SORT_INDEX_CASE(U64);
+        PSVECTOR_SORT_INDEX_CASE(S8);
+        PSVECTOR_SORT_INDEX_CASE(S16);
+        PSVECTOR_SORT_INDEX_CASE(S32);
+        PSVECTOR_SORT_INDEX_CASE(S64);
+        PSVECTOR_SORT_INDEX_CASE(F32);
+        PSVECTOR_SORT_INDEX_CASE(F64);
     default:
         psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                 PS_ERRORTEXT_psVector_UNSUPPORTED_TYPE,
-                inType);
+                inVector->type.type);
         psFree(outVector);
         return NULL;
     }
-
-    return outVector;
-}
-
-psVector* psVectorSortIndex(psVector* outVector,
-                            const psVector* inVector)
-{
-    psS32 N = 0;
-    psElemType inType = 0;
-
-    if (inVector == NULL) {
-        psError(PS_ERR_BAD_PARAMETER_NULL, true,
-                PS_ERRORTEXT_psVector_SORT_NULL);
-        psFree(outVector);
-        return NULL;
-    }
-
-    inType = inVector->type.type;
-    N = inVector->n;
-
-    if (N == 0) {
-        // no need to sort anything, as there are no elements in input vector.
-        return outVector;
-    }
-
-    // ok, let's create a temporary indexed vector
-    indexedVector* idxVector = psAlloc(sizeof(indexedVector)*N);
-    int elSize = PSELEMTYPE_SIZEOF(inType);
-    for (int i = 0; i < N; i++) {
-        idxVector[i].data.U8 = inVector->data.U8+i*elSize;
-        idxVector[i].index = i;
-    }
-
-    // Sort indexed vector
-    // n.b., since first element in indexedVector is a pointer to the data,
-    // we can use the 'Ptr' version of the standard compare functions
-    switch (inType) {
-    case PS_TYPE_U8:
-        qsort(idxVector, N, sizeof(indexedVector), (psCompareFcn)psCompareU8Ptr);
-        break;
-    case PS_TYPE_U16:
-        qsort(idxVector, N, sizeof(indexedVector), (psCompareFcn)psCompareU16Ptr);
-        break;
-    case PS_TYPE_U32:
-        qsort(idxVector, N, sizeof(indexedVector), (psCompareFcn)psCompareU32Ptr);
-        break;
-    case PS_TYPE_U64:
-        qsort(idxVector, N, sizeof(indexedVector), (psCompareFcn)psCompareU64Ptr);
-        break;
-    case PS_TYPE_S8:
-        qsort(idxVector, N, sizeof(indexedVector), (psCompareFcn)psCompareS8Ptr);
-        break;
-    case PS_TYPE_S16:
-        qsort(idxVector, N, sizeof(indexedVector), (psCompareFcn)psCompareS16Ptr);
-        break;
-    case PS_TYPE_S32:
-        qsort(idxVector, N, sizeof(indexedVector), (psCompareFcn)psCompareS32Ptr);
-        break;
-    case PS_TYPE_S64:
-        qsort(idxVector, N, sizeof(indexedVector), (psCompareFcn)psCompareS64Ptr);
-        break;
-    case PS_TYPE_F32:
-        qsort(idxVector, N, sizeof(indexedVector), (psCompareFcn)psCompareF32Ptr);
-        break;
-    case PS_TYPE_F64:
-        qsort(idxVector, N, sizeof(indexedVector), (psCompareFcn)psCompareF64Ptr);
-        break;
-    default:
-        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
-                PS_ERRORTEXT_psVector_UNSUPPORTED_TYPE,
-                inType);
-        psFree(idxVector);
-        psFree(outVector);
-        return NULL;
-    }
-
-    // extract the indices to the output vector
-    outVector = psVectorRecycle(outVector, N, PS_TYPE_U32);
-    outVector->n = N;
-    psU32* outData = outVector->data.U32;
-    for (int i = 0; i < N; i++) {
-        outData[i] = idxVector[i].index;
-    }
-
-    // Free temp memory
-    psFree(idxVector);
-
-    return outVector;
+    return out;
 }