mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
Move math functions to separate source
This commit is contained in:
parent
bee46316c4
commit
3a18b8f4b6
133
src/img/fmath.c
Normal file
133
src/img/fmath.c
Normal file
@ -0,0 +1,133 @@
|
||||
#include "mdefs.h"
|
||||
#include "fmath.h"
|
||||
#define M_PI_2 1.57079632f
|
||||
#define M_PI_4 0.78539816f
|
||||
|
||||
const float __atanf_lut[4] = {
|
||||
-0.0443265554792128, //p7
|
||||
-0.3258083974640975, //p3
|
||||
+0.1555786518463281, //p5
|
||||
+0.9997878412794807 //p1
|
||||
};
|
||||
|
||||
float ALWAYS_INLINE fast_sqrtf(float x)
|
||||
{
|
||||
asm volatile (
|
||||
"vsqrt.f32 %[r], %[x]\n"
|
||||
: [r] "=t" (x)
|
||||
: [x] "t" (x));
|
||||
return x;
|
||||
}
|
||||
|
||||
int ALWAYS_INLINE fast_floorf(float x)
|
||||
{
|
||||
int i;
|
||||
asm volatile (
|
||||
"vcvt.S32.f32 %[r], %[x]\n"
|
||||
: [r] "=t" (i)
|
||||
: [x] "t" (x));
|
||||
return i;
|
||||
}
|
||||
|
||||
int ALWAYS_INLINE fast_roundf(float x)
|
||||
{
|
||||
int i;
|
||||
asm volatile (
|
||||
"vcvtr.s32.f32 %[r], %[x]\n"
|
||||
: [r] "=t" (i)
|
||||
: [x] "t" (x));
|
||||
return i;
|
||||
}
|
||||
|
||||
#if 0
|
||||
float ALWAYS_INLINE fast_atanf(float x)
|
||||
{
|
||||
//return (0.97239f * x) - (0.19195f * x*x*x);
|
||||
return M_PI_4*x - x*(fast_fabsf(x) - 1.0f)*(0.2447f + 0.0663f*fast_fabsf(x));
|
||||
}
|
||||
#endif
|
||||
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wstrict-aliasing"
|
||||
typedef union{
|
||||
uint32_t l;
|
||||
struct {
|
||||
uint32_t m : 20;
|
||||
uint32_t e : 11;
|
||||
uint32_t s : 1;
|
||||
};
|
||||
}exp_t;
|
||||
|
||||
float fast_expf(float x)
|
||||
{
|
||||
exp_t e;
|
||||
e.l = (uint32_t)(1512775 * x + 1072632447);
|
||||
// IEEE binary32 format
|
||||
e.e = (e.e -1023 + 127) &0xFF; // rebase
|
||||
|
||||
uint32_t packed = (e.s << 31) | (e.e << 23) | e.m <<3;
|
||||
return *((float*)&packed);
|
||||
}
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
float fast_cbrtf(float f)
|
||||
{
|
||||
unsigned int* p = (unsigned int *) &f;
|
||||
*p = *p/3 + 709921077;
|
||||
return f;
|
||||
}
|
||||
|
||||
float ALWAYS_INLINE fast_fabsf(float x)
|
||||
{
|
||||
asm volatile (
|
||||
"vabs.f32 %[r], %[x]\n"
|
||||
: [r] "=t" (x)
|
||||
: [x] "t" (x));
|
||||
return x;
|
||||
}
|
||||
|
||||
float fast_atanf(float x)
|
||||
{
|
||||
|
||||
float a, b, r, xx;
|
||||
int m;
|
||||
|
||||
union {
|
||||
float f;
|
||||
int i;
|
||||
} xinv, ax;
|
||||
|
||||
ax.f = fast_fabsf(x);
|
||||
|
||||
//fast inverse approximation (2x newton)
|
||||
xinv.f = ax.f;
|
||||
m = 0x3F800000 - (xinv.i & 0x7F800000);
|
||||
xinv.i = xinv.i + m;
|
||||
xinv.f = 1.41176471f - 0.47058824f * xinv.f;
|
||||
xinv.i = xinv.i + m;
|
||||
b = 2.0 - xinv.f * ax.f;
|
||||
xinv.f = xinv.f * b;
|
||||
b = 2.0 - xinv.f * ax.f;
|
||||
xinv.f = xinv.f * b;
|
||||
|
||||
//if |x| > 1.0 -> ax = -1/ax, r = pi/2
|
||||
xinv.f = xinv.f + ax.f;
|
||||
a = (ax.f > 1.0f);
|
||||
ax.f = ax.f - a * xinv.f;
|
||||
r = a * M_PI_2;
|
||||
|
||||
//polynomial evaluation
|
||||
xx = ax.f * ax.f;
|
||||
a = (__atanf_lut[0] * ax.f) * xx + (__atanf_lut[2] * ax.f);
|
||||
b = (__atanf_lut[1] * ax.f) * xx + (__atanf_lut[3] * ax.f);
|
||||
xx = xx * xx;
|
||||
b = b + a * xx;
|
||||
r = r + b;
|
||||
|
||||
//if x < 0 -> r = -r
|
||||
a = 2 * r;
|
||||
b = (x < 0.0f);
|
||||
r = r - a * b;
|
||||
|
||||
return r;
|
||||
}
|
||||
11
src/img/fmath.h
Normal file
11
src/img/fmath.h
Normal file
@ -0,0 +1,11 @@
|
||||
#ifndef __FMATH_H__
|
||||
#define __FMATH_H__
|
||||
#include <stdint.h>
|
||||
float fast_sqrtf(float x);
|
||||
int fast_floor(float x);
|
||||
int fast_roundf(float x);
|
||||
float fast_atanf(float x);
|
||||
float fast_expf(float x);
|
||||
float fast_cbrtf(float d);
|
||||
float fast_fabsf(float d);
|
||||
#endif /* __FMATH_H__ */
|
||||
100
src/img/imlib.c
100
src/img/imlib.c
@ -6,7 +6,7 @@
|
||||
#include "imlib.h"
|
||||
#include "ff.h"
|
||||
#include "xalloc.h"
|
||||
|
||||
#include "mdefs.h"
|
||||
#define MIN(a,b) \
|
||||
({ __typeof__ (a) _a = (a); \
|
||||
__typeof__ (b) _b = (b); \
|
||||
@ -62,83 +62,6 @@ const uint8_t xyz_table[256]= {
|
||||
93.868573, 94.730654, 95.597335, 96.468625, 97.344529, 98.225055, 99.110210, 100.000000,
|
||||
};
|
||||
|
||||
float fast_sqrtf(float x)
|
||||
{
|
||||
asm volatile (
|
||||
"vsqrt.f32 %[r], %[x]\n"
|
||||
: [r] "=t" (x)
|
||||
: [x] "t" (x));
|
||||
return x;
|
||||
}
|
||||
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wstrict-aliasing"
|
||||
/* This is slightly faster the VSQRT */
|
||||
uint32_t fast_sqrt(float x)
|
||||
{
|
||||
uint32_t i = *(uint32_t*) &x;
|
||||
// adjust bias
|
||||
i += 127 << 23;
|
||||
// approximation of square root
|
||||
i >>= 1;
|
||||
return (uint32_t) *(float*) &i;
|
||||
}
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
int fast_floorf(float x)
|
||||
{
|
||||
int i;
|
||||
asm volatile (
|
||||
"vcvt.S32.f32 %[r], %[x]\n"
|
||||
: [r] "=t" (i)
|
||||
: [x] "t" (x));
|
||||
return i;
|
||||
}
|
||||
|
||||
int fast_roundf(float x)
|
||||
{
|
||||
int i;
|
||||
asm volatile (
|
||||
"vcvtr.s32.f32 %[r], %[x]\n"
|
||||
: [r] "=t" (i)
|
||||
: [x] "t" (x));
|
||||
return i;
|
||||
|
||||
// return (int) floor(x+0.5f);
|
||||
}
|
||||
|
||||
float fast_atanf(float x)
|
||||
{
|
||||
return (0.97239f * x) - (0.19195f * x*x*x);
|
||||
}
|
||||
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wstrict-aliasing"
|
||||
float fast_expf(float x)
|
||||
{
|
||||
double d;
|
||||
*((int*)(&d) + 0) = 0;
|
||||
*((int*)(&d) + 1) = (int)(1512775 * x + 1072632447);
|
||||
return (float) d;
|
||||
}
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
float fast_cbrtf(float f)
|
||||
{
|
||||
unsigned int* p = (unsigned int *) &f;
|
||||
*p = *p/3 + 709921077;
|
||||
return f;
|
||||
}
|
||||
|
||||
float fast_fabsf(float x)
|
||||
{
|
||||
asm volatile (
|
||||
"vabs.f32 %[r], %[x]\n"
|
||||
: [r] "=t" (x)
|
||||
: [x] "t" (x));
|
||||
return x;
|
||||
}
|
||||
|
||||
uint32_t imlib_lab_distance(struct color *c0, struct color *c1)
|
||||
{
|
||||
uint32_t sum=0;
|
||||
@ -311,12 +234,13 @@ void imlib_threshold(image_t *image, struct color *color, int threshold)
|
||||
lab2.B = lab_table[pixels[i]*3+2];
|
||||
/* add pixel if within threshold */
|
||||
if (imlib_lab_distance(&lab1, &lab2)<threshold) {
|
||||
pixels[i] = 0xFFFF;
|
||||
image->pixels[i] = 0x01;
|
||||
} else {
|
||||
pixels[i] = 0x0000;
|
||||
image->pixels[i] = 0x00;
|
||||
}
|
||||
}
|
||||
}
|
||||
image->bpp = 1;
|
||||
}
|
||||
|
||||
int imlib_image_mean(struct image *src)
|
||||
@ -395,10 +319,10 @@ void imlib_draw_rectangle(struct image *image, struct rectangle *r)
|
||||
{
|
||||
int i;
|
||||
uint8_t c=0xFF;
|
||||
int x = MIN(MAX(r->x, 1), image->w)-1;
|
||||
int y = MIN(MAX(r->y, 1), image->h)-1;
|
||||
int w = (x+r->w) >= image->w ? (image->w-x):r->w;
|
||||
int h = (y+r->h) >= image->h ? (image->h-y):r->h;
|
||||
int x = MIN(MAX(r->x, 0), image->w-1);
|
||||
int y = MIN(MAX(r->y, 0), image->h-1);
|
||||
int w = (x+r->w) >= image->w ? (image->w-x-1):r->w;
|
||||
int h = (y+r->h) >= image->h ? (image->h-y-1):r->h;
|
||||
|
||||
x *= image->bpp;
|
||||
w *= image->bpp;
|
||||
@ -417,6 +341,14 @@ void imlib_draw_rectangle(struct image *image, struct rectangle *r)
|
||||
image->pixels[(y+i)*col + x + w+1] = c;
|
||||
}
|
||||
}
|
||||
|
||||
if (image->bpp>1) {
|
||||
for (i=0; i<h; i++) {
|
||||
image->pixels[(y+i)*col + x+1] = c;
|
||||
image->pixels[(y+i)*col + x + w+1] = c;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void imlib_histeq(struct image *src)
|
||||
|
||||
@ -3,6 +3,7 @@
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "array.h"
|
||||
#include "fmath.h"
|
||||
typedef struct point {
|
||||
uint16_t x;
|
||||
uint16_t y;
|
||||
@ -152,16 +153,6 @@ typedef struct cascade {
|
||||
int8_t *rectangles_array;
|
||||
} cascade_t;
|
||||
|
||||
/* Basic math functions */
|
||||
float fast_sqrtf(float x);
|
||||
uint32_t fast_sqrt(float x);
|
||||
int fast_floor(float x);
|
||||
int fast_roundf(float x);
|
||||
float fast_atanf(float x);
|
||||
float fast_expf(float x);
|
||||
float fast_cbrtf(float d);
|
||||
float fast_fabsf(float d);
|
||||
|
||||
/* Point functions */
|
||||
point_t *point_alloc(int x, int y);
|
||||
int point_equal(point_t *p1, point_t *p2);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user