From 3a5a95e2bdb38052a1047ae1490f29a8a14a8bc2 Mon Sep 17 00:00:00 2001 From: martin-s Date: Sat, 27 Dec 2008 22:12:39 +0000 Subject: [PATCH] Add:Core:Support for hog and roll in transform git-svn-id: https://navit.svn.sourceforge.net/svnroot/navit/trunk@1857 ffa7fe5e-494d-0410-b361-a75ebd5db220 --- navit/navit/transform.c | 85 ++++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 77 insertions(+), 8 deletions(-) diff --git a/navit/navit/transform.c b/navit/navit/transform.c index 14e3467..e4b4acb 100644 --- a/navit/navit/transform.c +++ b/navit/navit/transform.c @@ -33,15 +33,21 @@ #include "point.h" #define POST_SHIFT 8 +#define ENABLE_ROLL struct transformation { int yaw; /* Rotation angle */ int pitch; - int roll; int ddd; int m00,m01,m10,m11; /* 2d transformation matrix */ int xyscale; int m20,m21; /* additional 3d parameters */ +#ifdef ENABLE_ROLL + int roll; + int m02,m12,m22; + int hog; + double im02,im12,im20,im21,im22; +#endif double im00,im01,im10,im11; /* inverse 2d transformation matrix */ struct map_selection *map_sel; struct map_selection *screen_sel; @@ -62,8 +68,12 @@ transform_setup_matrix(struct transformation *t) double fac; double yawc=cos(M_PI*t->yaw/180); double yaws=sin(M_PI*t->yaw/180); - double pitchc=cos(M_PI*t->pitch/180); - double pitchs=sin(M_PI*t->pitch/180); + double pitchc=cos(-M_PI*t->pitch/180); + double pitchs=sin(-M_PI*t->pitch/180); +#ifdef ENABLE_ROLL + double rollc=cos(M_PI*t->roll/180); + double rolls=sin(M_PI*t->roll/180); +#endif int scale=t->scale; int order_dir=-1; @@ -86,12 +96,24 @@ transform_setup_matrix(struct transformation *t) fac=(1 << POST_SHIFT) * (1 << t->scale_shift) / t->scale; dbg(1,"scale_shift=%d order=%d scale=%f fac=%f\n", t->scale_shift, t->order,t->scale,fac); +#ifdef ENABLE_ROLL + t->m00=rollc*yawc*fac; + t->m01=rollc*yaws*fac; + t->m02=-rolls*fac; + t->m10=(pitchs*rolls*yawc-pitchc*yaws)*(-fac); + t->m11=(pitchs*rolls*yaws+pitchc*yawc)*(-fac); + t->m12=pitchs*rollc*(-fac); + t->m20=(pitchc*rolls*yawc+pitchs*yaws)*fac; + t->m21=(pitchc*rolls*yaws-pitchs*yawc)*fac; + t->m22=pitchc*rollc*fac; +#else t->m00=yawc*fac; t->m01=-yaws*fac; t->m10=-pitchc*yaws*fac; t->m11=-pitchc*yawc*fac; t->m20=pitchs*yaws*fac; t->m21=pitchs*yawc*fac; +#endif t->offz=0; t->xyscale=1; t->ddd=0; @@ -102,12 +124,30 @@ transform_setup_matrix(struct transformation *t) t->offz=t->screen_dist; t->xyscale=t->offz; } +#ifdef ENABLE_ROLL + det=(double)t->m00*(double)t->m11*(double)t->m22+ + (double)t->m01*(double)t->m12*(double)t->m20+ + (double)t->m02*(double)t->m10*(double)t->m21- + (double)t->m02*(double)t->m11*(double)t->m20- + (double)t->m01*(double)t->m10*(double)t->m22- + (double)t->m00*(double)t->m12*(double)t->m21; + + t->im00=(t->m11*t->m22-t->m12*t->m21)/det; + t->im01=(t->m02*t->m21-t->m01*t->m22)/det; + t->im02=(t->m01*t->m12-t->m02*t->m11)/det; + t->im10=(t->m12*t->m20-t->m10*t->m22)/det; + t->im11=(t->m00*t->m22-t->m02*t->m20)/det; + t->im12=(t->m02*t->m10-t->m00*t->m12)/det; + t->im20=(t->m10*t->m21-t->m11*t->m20)/det; + t->im21=(t->m01*t->m20-t->m00*t->m21)/det; + t->im22=(t->m00*t->m11-t->m01*t->m10)/det; +#else det=((double)t->m00*(double)t->m11-(double)t->m01*(double)t->m10)*t->xyscale; - dbg(1,"det=%f\n", det); t->im00=t->m11/det; t->im01=-t->m01/det; t->im10=-t->m10/det; t->im11=t->m00/det; +#endif } struct transformation * @@ -120,6 +160,10 @@ transform_new(void) #if 0 this_->pitch=20; #endif +#if 0 + this_->roll=30; + this_->hog=1000; +#endif transform_setup_matrix(this_); return this_; } @@ -248,11 +292,20 @@ transform(struct transformation *t, enum projection pro, struct coord *c, struct yc-=t->map_center.y; xc >>= t->scale_shift; yc >>= t->scale_shift; +#ifdef ENABLE_ROLL + xcn=xc*t->m00+yc*t->m01+t->hog*t->m02; + ycn=xc*t->m10+yc*t->m11+t->hog*t->m12; +#else xcn=xc*t->m00+yc*t->m01; ycn=xc*t->m10+yc*t->m11; +#endif if (t->ddd) { +#ifdef ENABLE_ROLL + zc=(xc*t->m20+yc*t->m21+t->hog*t->m22); +#else zc=(xc*t->m20+yc*t->m21); +#endif zct=zc; zc+=t->offz << POST_SHIFT; dbg(1,"zc=%d\n", zc); @@ -325,21 +378,37 @@ void transform_reverse(struct transformation *t, struct point *p, struct coord *c) { double zc,xc,yc,xcn,ycn,q; + double offz=t->offz << POST_SHIFT; xc=p->x - t->offx; yc=p->y - t->offy; if (t->ddd) { + xc/=t->xyscale; + yc/=t->xyscale; double f00=xc*t->im00*t->m20; double f01=yc*t->im01*t->m20; double f10=xc*t->im10*t->m21; double f11=yc*t->im11*t->m21; - q=(1-f00-f01-f10-f11); +#ifdef ENABLE_ROLL + q=(1-f00-f01-t->im02*t->m20-f10-f11-t->im12*t->m21); if (q < 0) q=0.15; - zc=(t->offz << POST_SHIFT)*(1+(f00+f01+f10+f11)/q); - xcn=xc*zc; - ycn=yc*zc; + zc=(offz*((f00+f01+f10+f11))+t->hog*t->m22)/q; +#else + q=(1-f00-f01-f10-f11); + if (q < 0) + q=0.15; + zc=offz*(f00+f01+f10+f11)/q; +#endif + xcn=xc*(zc+offz); + ycn=yc*(zc+offz); +#ifdef ENABLE_ROLL + xc=xcn*t->im00+ycn*t->im01+zc*t->im02; + yc=xcn*t->im10+ycn*t->im11+zc*t->im12; +#else xc=xcn*t->im00+ycn*t->im01; yc=xcn*t->im10+ycn*t->im11; +#endif + } else { xcn=xc; ycn=yc; -- 2.7.4