/****************************************************************************** * CBsp-Int.c - Bspline curve interpolation/approximation schemes. * ******************************************************************************* * (C) Gershon Elber, Technion, Israel Institute of Technology * ******************************************************************************* * Written by Gershon Elber, Feb. 94. * ******************************************************************************/ #include #include #include #include "cagd_loc.h" #include "extra_fn.h" /***************************************************************************** * DESCRIPTION: M * Given a set of points, PtList, computes a Bspline curve of order Order M * that interpolates or least square approximates the set of points. M * The size of the control polygon of the resulting Bspline curve defaults M * to the number of points in PtList (if CrvSize = 0). M * However, this number is can smaller to yield a least square M * approximation. M * The created curve can be parametrized as specified by ParamType. M * * * PARAMETERS: M * PtList: List of points to interpolate/least square approximate. M * Order: Of interpolating/approximating curve. M * CrvSize: Number of degrees of freedom (control points) of the M * interpolating/approximating curve. M * ParamType: Type of parametrization. M * Periodic: Constructed curve should be Periodic. Periodic M * necessitates uniform knot sequence in ParamType. M * * * RETURN VALUE: M * CagdCrvStruct *: Constructed interpolating/approximating curve. M * * * KEYWORDS: M * BspCrvInterpPts, interpolation, least square approximation M *****************************************************************************/ CagdCrvStruct *BspCrvInterpPts(CagdPtStruct *PtList, int Order, int CrvSize, CagdParametrizationType ParamType, CagdBType Periodic) { int i, NumPts = CagdListLength(PtList), CrvPerSize = CrvSize + (Periodic ? Order - 1 : 0); CagdPtStruct *Pt; CagdRType *KV, *PtKnots, *R, *R2, t; CagdCrvStruct *Crv; CagdCtlPtStruct *CtlPt = NULL, *CtlPtList = NULL; if (CrvSize == 0) { CrvSize = NumPts; CrvPerSize = CrvSize + (Periodic ? Order - 1 : 0); } if (NumPts < 2 || NumPts < Order || CrvSize < Order) return NULL; R = PtKnots = (CagdRType *) IritMalloc(sizeof(CagdRType) * NumPts); if (Periodic) ParamType = CAGD_UNIFORM_PARAM; /* Compute parameter values at which the curve will interpolate PtList. */ switch (ParamType) { case CAGD_CHORD_LEN_PARAM: *R++ = 0.0; for (Pt = PtList; Pt -> Pnext != NULL; Pt = Pt -> Pnext, R++) *R = *(R - 1) + IRIT_EPS + PT_PT_DIST(Pt -> Pt, Pt -> Pnext -> Pt); for (t = R[-1]; R != PtKnots; *--R /= t); break; case CAGD_CENTRIPETAL_PARAM: *R++ = 0.0; for (Pt = PtList; Pt -> Pnext != NULL; Pt = Pt -> Pnext, R++) *R = *(R - 1) + IRIT_EPS + sqrt(PT_PT_DIST(Pt -> Pt, Pt -> Pnext -> Pt)); for (t = R[-1]; R != PtKnots; *--R /= t); break; case CAGD_UNIFORM_PARAM: default: /* For periodic cases, last point is the same as first point */ /* so we better not put different conflicting constraint there. */ for (i = 0; i < NumPts; i++) *R++ = Periodic ? ((RealType) i) / NumPts : ((RealType) i) / (NumPts - 1); break; } /* Construct the knot vector of the Bspline curve. */ KV = (CagdRType *) IritMalloc(sizeof(CagdRType) * (CrvPerSize + Order)); if (Periodic) { for (i = CrvPerSize + Order - 1; i >= 0; i--) KV[i] = (i - Order + 1) / ((RealType) CrvSize); } else if (CrvSize <= NumPts) { /* Overconstraint problem. */ CagdRType *AveKV = BspKnotAverage(PtKnots, NumPts, NumPts + Order - CrvSize - 1); BspKnotAffineTrans(AveKV, CrvSize - Order + 2, PtKnots[0] - AveKV[0], (PtKnots[NumPts - 1] - PtKnots[0]) / (AveKV[CrvSize - Order + 1] - AveKV[0])); for (i = 0, R = KV, R2 = AveKV; i < Order; i++) *R++ = *R2; for (i = 0; i < CrvSize - Order; i++) *R++ = *++R2; for (i = 0, R2++; i < Order; i++) *R++ = *R2; IritFree((VoidPtr) AveKV); } else { /* Under constraint problem. */ CagdRType r, Index; for (i = 0, R = KV, R2 = PtKnots; i < Order; i++) *R++ = *R2; r = ((RealType) (NumPts - 1)) / (1 + CrvSize - Order); for (i = 0, Index = r; i < CrvSize - Order; i++, Index += r) { CagdRType RIndex = BOUND(Index, 0, NumPts - 1 - IRIT_UEPS); int IIndex = (int) RIndex; CagdRType Frac = RIndex - IIndex; *R++ = PtKnots[IIndex] * (1.0 - Frac) + PtKnots[IIndex + 1] * Frac; } for (i = 0, R2 = &PtKnots[NumPts - 1]; i < Order; i++) *R++ = *R2; } /* Convert to control points in a linear list */ for (Pt = PtList; Pt != NULL; Pt = Pt -> Pnext) { if (CtlPtList == NULL) CtlPtList = CtlPt = CagdCtlPtNew(CAGD_PT_E3_TYPE); else { CtlPt -> Pnext = CagdCtlPtNew(CAGD_PT_E3_TYPE); CtlPt = CtlPt -> Pnext; } for (i = 0; i < 3; i++) CtlPt -> Coords[i + 1] = Pt -> Pt[i]; } Crv = BspCrvInterpolate(CtlPtList, NumPts, PtKnots, KV, CrvSize, Order, Periodic); CagdCtlPtFreeList(CtlPtList); IritFree((VoidPtr *) PtKnots); IritFree((VoidPtr *) KV); return Crv; } /***************************************************************************** * DESCRIPTION: M * Given set of points, PtList, parameter values the curve should interpolate M * or approximate these points, Params, and the expected knot vector, KV, M * length Length and order Order of the Bspline curve, computes the Bspline M * curve's coefficients. M * All points in PtList are assumed of the same type. M * If Periodic, Order - 1 more constraints (and DOF's) are added so that M * the first Order - 1 points are the same as the last Order - 1 points. M * * * PARAMETERS: M * PtList: List of points to interpolate/least square approximate. M * NumPts: Size of PtList. M * Params: At which to interpolate the points in PtList. M * KV: Computed knot vector for the constructed curve. M * Length: Number of degrees of freedom (control points) of the M * interpolating/approximating curve. M * Order: Of interpolating/approximating curve. M * Periodic: Constructed curve should be Periodic. M * * * RETURN VALUE: M * CagdCrvStruct *: Constructed interpolating/approximating curve. M * * * KEYWORDS: M * BspCrvInterpolate, interpolation, least square approximation M *****************************************************************************/ CagdCrvStruct *BspCrvInterpolate(CagdCtlPtStruct *PtList, int NumPts, CagdRType *Params, CagdRType *KV, int Length, int Order, CagdBType Periodic) { int i, j, k; CagdCtlPtStruct *Pt; CagdRType *Mat, *InterpPts, *M, *R; CagdCrvStruct *Crv; CagdPointType PtType = PtList -> PtType; /* Construct the Bspline curve and its knot vector. */ Crv = BspPeriodicCrvNew(Length, Order, Periodic, PtType); CAGD_GEN_COPY(Crv -> KnotVector, KV, (CAGD_CRV_PT_LST_LEN(Crv) + Order) * sizeof(CagdRType)); /* Construct the system of equations' matrix. */ M = Mat = (CagdRType *) IritMalloc(sizeof(CagdRType) * Length * MAX(NumPts, Length)); for (i = 0, R = Params; i < NumPts; i++, R++, M += Length) { int Index; CagdRType *Line = BspCrvCoxDeBoorBasis(KV, Order, Length + (Periodic ? Order - 1 : 0), *R, &Index); for (k = 0; k < Length; k++) M[k] = 0.0; for (j = Order, k = Index; j > 0; j--, k++) M[k % Length] = *Line++; } /* Augment the rest of the lines with zeros. */ for (i = NumPts; i < Length; i++, M += Length) { for (k = 0; k < Length; k++) M[k] = 0.0; } # ifdef DEBUG_PRINT_INPUT_SVD for (i = 0; i < MAX(NumPts, Length); i++) { fprintf(stderr, "["); for (j = 0; j < Length; j++) { fprintf(stderr, "%7.4f ", Mat[i * Length + j]); } fprintf(stderr, "]\n"); } # endif /* DEBUG_PRINT_INPUT_SVD */ /* Compute SVD decomposition for Mat. */ if (fabs(SvdLeastSqr(Mat, NULL, NULL, MAX(NumPts, Length), Length)) < IRIT_UEPS && Length <= NumPts) { CAGD_FATAL_ERROR(CAGD_ERR_NO_SOLUTION); IritFree((VoidPtr *) Mat); CagdCrvFree(Crv); return NULL; } IritFree((VoidPtr *) Mat); /* Solve for the coefficients of all the coordinates of the curve. */ InterpPts = (CagdRType *) IritMalloc(sizeof(CagdRType) * MAX(NumPts, Length)); for (i = !CAGD_IS_RATIONAL_PT(PtType); i <= CAGD_NUM_OF_PT_COORD(PtType); i++) { for (Pt = PtList, R = InterpPts, j = NumPts; j > 0; Pt = Pt -> Pnext, j--) *R++ = Pt -> Coords[i]; SvdLeastSqr(NULL, Crv -> Points[i], InterpPts, NumPts, Length); } IritFree((VoidPtr *) InterpPts); return Crv; } /***************************************************************************** * DESCRIPTION: M * Given a set of points, and a curve least square fitting them using the M * BspCrvInterpPts function, computes an error measure as a the maximal M * distance between the curve and points (L1 norm). M * * * PARAMETERS: M * Crv: Curve that was fitted to the data set. M * PtList: The data set. M * ParamType: Parameter values at with curve should interpolate PtList. M * Periodic: Constructed curve should be Periodic. Periodic M * necessitates uniform knot sequence in ParamType. M * * * RETURN VALUE: M * CagdRType: Error measured in the L1 norm. M * * * KEYWORDS: M * BspCrvInterpPtsError, error estimation, interpolation, least square M * approximation M *****************************************************************************/ CagdRType BspCrvInterpPtsError(CagdCrvStruct *Crv, CagdPtStruct *PtList, CagdParametrizationType ParamType, CagdBType Periodic) { int i, NumPts; CagdPtStruct *Pt; CagdRType *PtKnots, *R, t, MaxDist = 0.0; for (NumPts = 0, Pt = PtList; Pt != NULL; NumPts++, Pt = Pt -> Pnext); R = PtKnots = (CagdRType *) IritMalloc(sizeof(CagdRType) * NumPts); if (Periodic) ParamType = CAGD_UNIFORM_PARAM; /* Compute parameter values at which the curve interpolated PtList. */ switch (ParamType) { case CAGD_CHORD_LEN_PARAM: *R++ = 0.0; for (Pt = PtList; Pt -> Pnext != NULL; Pt = Pt -> Pnext, R++) *R = *(R - 1) + PT_PT_DIST(Pt -> Pt, Pt -> Pnext -> Pt); for (t = R[-1]; R != PtKnots; *--R /= t); break; case CAGD_CENTRIPETAL_PARAM: *R++ = 0.0; for (Pt = PtList; Pt -> Pnext != NULL; Pt = Pt -> Pnext, R++) *R = *(R - 1) + sqrt(PT_PT_DIST(Pt -> Pt, Pt -> Pnext -> Pt)); for (t = R[-1]; R != PtKnots; *--R /= t); break; case CAGD_UNIFORM_PARAM: default: for (i = 0; i < NumPts; i++) *R++ = ((RealType) i) / (NumPts - 1); break; } for (i = 0, Pt = PtList; i < NumPts; i++, Pt = Pt -> Pnext) { CagdRType Dist; R = CagdCrvEval(Crv, PtKnots[i]); R = &R[1]; Dist = PT_PT_DIST(R, Pt->Pt); if (Dist > MaxDist) MaxDist = Dist; } return MaxDist; } /***************************************************************************** * DESCRIPTION: M * Computes zero and first moments of a curve. M * * * PARAMETERS: M * Crv: To compute zero and first moment. M * n: Number of samples the curve should be sampled at. M * Loc: Center of curve as zero moment. M * Dir: Main direction of curve as first moment. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * CagdCrvFirstMoments M *****************************************************************************/ void CagdCrvFirstMoments(CagdCrvStruct *Crv, int n, CagdPType Loc, CagdVType Dir) { int i; CagdRType t, TMin, TMax, Dt, **Points; CagdPtStruct *Pt, *PtList = NULL; CagdCrvStruct *InterpCrv; if (n < 2) n = 2; CagdCrvDomain(Crv, &TMin, &TMax); t = TMin; Dt = (TMax - TMin) / (n - 1); for (i = 0; i < n; i++, t += Dt) { RealType *R = CagdCrvEval(Crv, t); Pt = CagdPtNew(); CagdCoerceToE3(Pt -> Pt, &R, -1, Crv -> PType); LIST_PUSH(Pt, PtList); } InterpCrv = BspCrvInterpPts(PtList, 2, 2, CAGD_UNIFORM_PARAM, Crv -> Periodic); CagdPtFreeList(PtList); Points = InterpCrv -> Points; Loc[0] = (Points[1][0] + Points[1][1]) / 2.0; Loc[1] = (Points[2][0] + Points[2][1]) / 2.0; Loc[2] = (Points[3][0] + Points[3][1]) / 2.0; Dir[0] = (Points[1][1] - Points[1][0]); Dir[1] = (Points[2][1] - Points[2][0]); Dir[2] = (Points[3][1] - Points[3][0]); PT_NORMALIZE(Dir); CagdCrvFree(InterpCrv); } /***************************************************************************** * DESCRIPTION: M * Computes a reparametrization scalar Bspline curve, y(x), so that each at M * each point in PtsList, the curve parameter value of X is evaluated into Y. M * * * PARAMETERS: M * PtsList: List of points on the reparametrization curve. M * Order: of reparametrization curve. M * DegOfFreedom: of reparametrization curve (== number of coefficients). M * * * RETURN VALUE: M * CagdCrvStruct *: Result of reparametrization curve, computed using M * list squares fit. M * * * SEE ALSO: M * BspCrvInterpolate M * * * KEYWORDS: M * BspMakeReparamCurve M *****************************************************************************/ CagdCrvStruct *BspMakeReparamCurve(CagdPtStruct *PtsList, int Order, int DegOfFreedom) { int i, j, Len = CagdListLength(PtsList); CagdRType r, *Pts, Min, Max; CagdPtStruct *Pt; CagdCtlPtStruct *PtE1List; CagdRType *Kv, Delta, *Params = (CagdRType *) IritMalloc(Len * sizeof(CagdRType)); CagdCrvStruct *Crv; Min = PtsList -> Pt[1]; for (Pt = PtsList, PtE1List = NULL, i = 0; Pt != NULL; Pt = Pt -> Pnext) { CagdCtlPtStruct *PtE1 = CagdCtlPtNew(CAGD_PT_E1_TYPE); Params[i++] = Pt -> Pt[0]; PtE1 -> Coords[1] = Max = Pt -> Pt[1]; LIST_PUSH(PtE1, PtE1List); } PtE1List = CagdListReverse(PtE1List); Delta = ((CagdRType) Len) / (DegOfFreedom - Order); if (DegOfFreedom <= Order || DegOfFreedom > Len || Delta < 2.0) { CagdCtlPtFreeList(PtE1List); IritFree(Params); CAGD_FATAL_ERROR(CAGD_ERR_WRONG_ORDER); return NULL; } Kv = (CagdRType *) IritMalloc((DegOfFreedom + Order) * sizeof(CagdRType)); for (i = j = 0; i < Order; i++, j++) Kv[j] = Params[0]; for (r = Delta / 2; j < DegOfFreedom; r += Delta) Kv[j++] = Params[(int) r]; for (i = 0; i < Order; i++) Kv[j++] = Params[Len - 1]; /* Interpolate a bspline curve from the points. */ Crv = BspCrvInterpolate(PtE1List, Len, Params, Kv, DegOfFreedom, Order, FALSE); CagdCtlPtFreeList(PtE1List); IritFree(Kv); IritFree(Params); /* Make sure the curve is monotone. */ Pts = Crv -> Points[1]; for (i = 1; i < Crv -> Length; i++) { if (Pts[i] < Pts[i - 1]) Pts[i] = Pts[i - 1] + IRIT_EPS; } /* Make sure the range spaned is proper. */ r = (Max - Min) / (Pts[Crv -> Length - 1] - Pts[0]); for (i = 1; i < Crv -> Length; i++) Pts[i] = (Pts[i] - Pts[0]) * r + Min; /* Return the new parametrization curve. */ return Crv; }