[ros-dev] [ros-diffs] [jimtabor] 26877: - Fixed arithmetic semantics.

[jimtabor]

Hi,
Here is a test program.
Thanks,
James



#include <stdio.h>
#include <stdbool.h>
#include <math.h>
#include <float.h>

//#define APIENTRY __stdcall
#define APIENTRY
//#define FASTCALL __fastcall
#define FASTCALL

#ifdef __GNUC__
#define FLOAT_TO_INT(in,out)  \
           __asm__ __volatile__ ("fistpl %0" : "=m" (out) :
"t" (in) : "st");
#else
#define FLOAT_TO_INT(in,out) \
          __asm fld in \
          __asm fistp out
#endif


/* the following deal with IEEE single-precision numbers */
#define EXCESS          126L
#define SIGNBIT         0x80000000L
#define SIGN(fp)        ((fp) & SIGNBIT)
#define EXP(fp)         (((fp) >> 23L) & 0xFF)
#define MANT(fp)        ((fp) & 0x7FFFFFL)
#define PACK(s,e,m)     ((s) | ((e) << 23L) | (m))
 

typedef union // float_long
{
    float f;
    unsigned long l;
    char c[4];
} gxf_long;

#ifdef _X86_
typedef long FLOATL;
#else /* !_X86_ */
typedef float FLOATL;
#endif

typedef struct _FLOATOBJ
{
  unsigned long  ul1;
  unsigned long  ul2;
} FLOATOBJ, *PFLOATOBJ;
    
typedef struct _EFLOAT_S
{
  long lMant;
  long lExp;
} EFLOAT_S;

////
////
void
FASTCALL
EF_Negate(EFLOAT_S * efp)
{
  efp->lMant = -efp->lMant;
}

long
FASTCALL
EFtoF( EFLOAT_S * efp)
{
 long Mant, Exp, Sign = 0;

 if (!efp->lMant) return 0;
 printf("EFtoF   %x : %x\n", efp->lMant, efp->lExp);

 Mant = efp->lMant;
 Exp = efp->lExp;
 Sign = SIGN(efp->lMant);
//// M$ storage emulation
 if( Sign ) Mant = -Mant;
 Mant = ((Mant & 0x3fffffff)>>7);
 Exp += (EXCESS-1);
////
 Mant = MANT(Mant);
 return PACK(Sign, Exp, Mant);
}

void
FASTCALL
FtoEF( EFLOAT_S * efp, FLOATL f)
{
 long Mant, Exp, Sign = 0;
 gxf_long worker;
 printf("FtoEF\n");
#ifdef _X86_
 worker.l = f; // It's a float stored in a long.
#else
 worker.f = f;
#endif

 Exp = EXP(worker.l);
 Mant = MANT(worker.l); 
 if (SIGN(worker.l)) Sign = -1;
//// M$ storage emulation
 Mant = ((Mant << 7) | 0x40000000);
 Mant ^= Sign; // invert/mask it
 Mant -= Sign; // sub it
 Exp -= (EXCESS-1);
//// 
 efp->lMant = Mant;
 efp->lExp = Exp;
}


void
APIENTRY
FLOATOBJ_Neg ( PFLOATOBJ pf )
{
 EFLOAT_S * efp = (EFLOAT_S *)pf;
 EF_Negate(efp);
}

long
APIENTRY
FLOATOBJ_GetFloat (
    PFLOATOBJ  pf
     )
{
 EFLOAT_S * efp = (EFLOAT_S *)pf;
 return EFtoF(efp);
}


void
APIENTRY
FLOATOBJ_SetFloat(
    PFLOATOBJ  pf,
    FLOATL  f
      )
{
 EFLOAT_S * efp = (EFLOAT_S *)pf;
 FtoEF( efp, f);
}

long
APIENTRY
FLOATOBJ_GetLong(
    PFLOATOBJ  pf
          )
{
 EFLOAT_S * efp = (EFLOAT_S *)pf;
 gxf_long f;
 long l;

 f.l = EFtoF( efp );
 FLOAT_TO_INT(f.f, l); // Let FPP handle it the fasty haxy way.

 return l;
}

void
APIENTRY
FLOATOBJ_SetLong(
     PFLOATOBJ pf,
     long  l
      )
{
 EFLOAT_S * efp = (EFLOAT_S *)pf;
 gxf_long f;
 f.f = (float) l; // Convert it now.
#ifdef _X86_
 FtoEF( efp, f.l );
#else
 FtoEF( efp, f.f );
#endif
}

long FT_FixedFromFloat(float f)
{
   short value = f;
   unsigned short fract = (f - value) * 0xFFFF;
   return (long)((long)value << 16 | (unsigned long)fract);
}

double F2F(PFLOATOBJ f)
{
 printf("F2F --> %x : %x\n", f->ul1, f->ul2);
 if(SIGN(f->ul1)) // negate mant
   return (double) -(-f->ul1 * pow(2,(double)f->ul2-32));
 else return (double) f->ul1 * pow(2,(double)f->ul2-32);
}

void
Test_It(float fx)
{
 FLOATOBJ pf;
 gxf_long f;

 f.f = fx;
 
 printf("Start 1 %f\n", f.f);
 printf("      2 %x\n", f.l);

#ifdef _X86_
 FLOATOBJ_SetFloat( &pf, f.l );
#else
 FLOATOBJ_SetFloat( &pf, f.f );
#endif
// FLOATOBJ_Neg ( &pf );

 f.l = FLOATOBJ_GetFloat ( &pf );
 printf("Fin   1 %f\n",f.f);
 printf("Fin   2 %x\n",f.l);

 printf("F2F --> %f\n\n",F2F( &pf ));
}

//
// gcc -Os fpms.c -o f -lm
// or
// gcc -Os -D_X86_ fpms.c -o f -lm
//
main()
{
 gxf_long f;

 f.f = 1.0;
 Test_It(f.f);

 f.f = -1.0;
 Test_It(f.f);

 f.f = 2.0;
 Test_It(f.f);

 f.f = -2.0;
 Test_It(f.f);

 f.f = 1.5;
 Test_It(f.f);

 f.f = -1.5;
 Test_It(f.f);

 f.f = 1235.0;
 Test_It(f.f);

 f.f = -1235.0;
 Test_It(f.f);

 f.f = 1235678.9; // Max it
 Test_It(f.f);

 f.f = -1235678.9;
 Test_It(f.f);

}