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 "imlib.h"
|
||||||
#include "ff.h"
|
#include "ff.h"
|
||||||
#include "xalloc.h"
|
#include "xalloc.h"
|
||||||
|
#include "mdefs.h"
|
||||||
#define MIN(a,b) \
|
#define MIN(a,b) \
|
||||||
({ __typeof__ (a) _a = (a); \
|
({ __typeof__ (a) _a = (a); \
|
||||||
__typeof__ (b) _b = (b); \
|
__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,
|
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 imlib_lab_distance(struct color *c0, struct color *c1)
|
||||||
{
|
{
|
||||||
uint32_t sum=0;
|
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];
|
lab2.B = lab_table[pixels[i]*3+2];
|
||||||
/* add pixel if within threshold */
|
/* add pixel if within threshold */
|
||||||
if (imlib_lab_distance(&lab1, &lab2)<threshold) {
|
if (imlib_lab_distance(&lab1, &lab2)<threshold) {
|
||||||
pixels[i] = 0xFFFF;
|
image->pixels[i] = 0x01;
|
||||||
} else {
|
} else {
|
||||||
pixels[i] = 0x0000;
|
image->pixels[i] = 0x00;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
image->bpp = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
int imlib_image_mean(struct image *src)
|
int imlib_image_mean(struct image *src)
|
||||||
@ -395,10 +319,10 @@ void imlib_draw_rectangle(struct image *image, struct rectangle *r)
|
|||||||
{
|
{
|
||||||
int i;
|
int i;
|
||||||
uint8_t c=0xFF;
|
uint8_t c=0xFF;
|
||||||
int x = MIN(MAX(r->x, 1), image->w)-1;
|
int x = MIN(MAX(r->x, 0), image->w-1);
|
||||||
int y = MIN(MAX(r->y, 1), image->h)-1;
|
int y = MIN(MAX(r->y, 0), image->h-1);
|
||||||
int w = (x+r->w) >= image->w ? (image->w-x):r->w;
|
int w = (x+r->w) >= image->w ? (image->w-x-1):r->w;
|
||||||
int h = (y+r->h) >= image->h ? (image->h-y):r->h;
|
int h = (y+r->h) >= image->h ? (image->h-y-1):r->h;
|
||||||
|
|
||||||
x *= image->bpp;
|
x *= image->bpp;
|
||||||
w *= 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;
|
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)
|
void imlib_histeq(struct image *src)
|
||||||
|
|||||||
@ -3,6 +3,7 @@
|
|||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include "array.h"
|
#include "array.h"
|
||||||
|
#include "fmath.h"
|
||||||
typedef struct point {
|
typedef struct point {
|
||||||
uint16_t x;
|
uint16_t x;
|
||||||
uint16_t y;
|
uint16_t y;
|
||||||
@ -152,16 +153,6 @@ typedef struct cascade {
|
|||||||
int8_t *rectangles_array;
|
int8_t *rectangles_array;
|
||||||
} cascade_t;
|
} 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 functions */
|
||||||
point_t *point_alloc(int x, int y);
|
point_t *point_alloc(int x, int y);
|
||||||
int point_equal(point_t *p1, point_t *p2);
|
int point_equal(point_t *p1, point_t *p2);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user