| /* |
| * Copyright 2018 Google Inc. |
| * |
| * Use of this source code is governed by a BSD-style license that can |
| * be found in the LICENSE file. |
| * |
| */ |
| |
| #include <stdlib.h> |
| #include <assert.h> |
| #include <math.h> |
| |
| // |
| // |
| // |
| |
| #include "transform_stack.h" |
| |
| // |
| // |
| // |
| |
| #define SKC_TRANSFORM_SUFFIX_EVAL(a) a |
| |
| #define SKC_TRANSFORM_SUFFIX_CONCAT(func) \ |
| SKC_TRANSFORM_SUFFIX_EVAL(func)##SKC_TRANSFORM_SUFFIX_EVAL(SKC_TRANSFORM_FLOAT_SUFFIX) |
| |
| // |
| // |
| // |
| |
| #define SKC_TRANSFORM_SIN(x) SKC_TRANSFORM_SUFFIX_CONCAT(sin)(x) |
| #define SKC_TRANSFORM_COS(x) SKC_TRANSFORM_SUFFIX_CONCAT(cos)(x) |
| #define SKC_TRANSFORM_TAN(x) SKC_TRANSFORM_SUFFIX_CONCAT(tan)(x) |
| #define SKC_TRANSFORM_RCP(x) (SKC_TRANSFORM_SUFFIX_CONCAT(1.0) / (x)) |
| |
| // |
| // |
| // |
| |
| union skc_transform_stack_3x3_u |
| { |
| skc_transform_float_t a9[9]; |
| |
| struct { |
| skc_transform_float_t sx; |
| skc_transform_float_t shx; |
| skc_transform_float_t tx; |
| |
| skc_transform_float_t shy; |
| skc_transform_float_t sy; |
| skc_transform_float_t ty; |
| |
| skc_transform_float_t w0; |
| skc_transform_float_t w1; |
| skc_transform_float_t w2; |
| }; |
| }; |
| |
| |
| struct skc_transform_stack |
| { |
| uint32_t size; |
| uint32_t count; |
| |
| skc_transform_weakref_t * weakrefs; |
| union skc_transform_stack_3x3_u * transforms; |
| }; |
| |
| // |
| // |
| // |
| |
| static |
| void |
| skc_transform_stack_3x3_u_copy(union skc_transform_stack_3x3_u * const __restrict dst, |
| union skc_transform_stack_3x3_u const * const __restrict src) |
| { |
| for (int32_t ii=0; ii<9; ii++) |
| dst->a9[ii] = src->a9[ii]; |
| } |
| |
| // |
| // C = A * B |
| // |
| |
| static |
| void |
| skc_transform_stack_3x3_u_multiply(union skc_transform_stack_3x3_u * const __restrict C, |
| union skc_transform_stack_3x3_u const * const __restrict A, |
| union skc_transform_stack_3x3_u const * const __restrict B) |
| { |
| C->sx = A->sx * B->sx + A->shx * B->shy + A->tx * B->w0; |
| C->shx = A->sx * B->shx + A->shx * B->sy + A->tx * B->w1; |
| C->tx = A->sx * B->tx + A->shx * B->ty + A->tx * B->w2; |
| |
| C->shy = A->shy * B->sx + A->sy * B->shy + A->ty * B->w0; |
| C->sy = A->shy * B->shx + A->sy * B->sy + A->ty * B->w1; |
| C->ty = A->shy * B->tx + A->sy * B->ty + A->ty * B->w2; |
| |
| C->w0 = A->w0 * B->sx + A->w1 * B->shy + A->w2 * B->w0; |
| C->w1 = A->w0 * B->shx + A->w1 * B->sy + A->w2 * B->w1; |
| C->w2 = A->w0 * B->tx + A->w1 * B->ty + A->w2 * B->w2; |
| } |
| |
| // |
| // |
| // |
| |
| static |
| void |
| skc_transform_stack_resize(struct skc_transform_stack * const ts, uint32_t const size) |
| { |
| ts->size = size; |
| ts->weakrefs = realloc(ts->weakrefs, size * sizeof(*ts->weakrefs)); |
| ts->transforms = realloc(ts->transforms,size * sizeof(*ts->transforms)); |
| } |
| |
| static |
| void |
| skc_transform_stack_ensure(struct skc_transform_stack * const ts) |
| { |
| if (ts->count < ts->size) |
| return; |
| |
| // increase by 50% and by at least 8 |
| skc_transform_stack_resize(ts,ts->size + max(ts->size/2,8)); |
| } |
| |
| // |
| // |
| // |
| |
| struct skc_transform_stack * |
| skc_transform_stack_create(uint32_t const size) |
| { |
| struct skc_transform_stack * ts = malloc(sizeof(*ts)); |
| |
| ts->size = size; |
| ts->count = 0; |
| |
| ts->transforms = NULL; |
| ts->weakrefs = NULL; |
| |
| skc_transform_stack_resize(ts,size); |
| |
| return ts; |
| } |
| |
| void |
| skc_transform_stack_release(struct skc_transform_stack * const ts) |
| { |
| free(ts->transforms); |
| free(ts->weakrefs); |
| |
| free(ts); |
| } |
| |
| // |
| // |
| // |
| |
| uint32_t |
| skc_transform_stack_save(struct skc_transform_stack * const ts) |
| { |
| return ts->count; |
| } |
| |
| void |
| skc_transform_stack_restore(struct skc_transform_stack * const ts, |
| uint32_t const restore) |
| { |
| ts->count = restore; |
| } |
| |
| // |
| // |
| // |
| |
| static |
| union skc_transform_stack_3x3_u * |
| skc_transform_stack_tos(struct skc_transform_stack * const ts) |
| { |
| return ts->transforms + ts->count - 1; |
| } |
| |
| // |
| // |
| // |
| |
| #if 0 |
| |
| void |
| skc_transform_to_f32a8(float f32a8[8], skc_transform_float_t const a9[9]) |
| { |
| if (a9[8] == 1.0) |
| { |
| f32a8[0] = (float)a9[0]; |
| f32a8[1] = (float)a9[1]; |
| f32a8[2] = (float)a9[2]; |
| f32a8[3] = (float)a9[3]; |
| f32a8[4] = (float)a9[4]; |
| f32a8[5] = (float)a9[5]; |
| f32a8[6] = (float)a9[6]; |
| f32a8[7] = (float)a9[7]; |
| } |
| else |
| { |
| skc_transform_float_t const rcp = 1.0 / a9[8]; |
| |
| f32a8[0] = (float)(a9[0] * rcp); |
| f32a8[1] = (float)(a9[1] * rcp); |
| f32a8[2] = (float)(a9[2] * rcp); |
| f32a8[3] = (float)(a9[3] * rcp); |
| f32a8[4] = (float)(a9[4] * rcp); |
| f32a8[5] = (float)(a9[5] * rcp); |
| f32a8[6] = (float)(a9[6] * rcp); |
| f32a8[7] = (float)(a9[7] * rcp); |
| } |
| } |
| |
| #endif |
| |
| // |
| // |
| // |
| |
| skc_transform_float_t * |
| skc_transform_stack_top_transform(struct skc_transform_stack * const ts) |
| { |
| return skc_transform_stack_tos(ts)->a9; |
| } |
| |
| skc_weakref_t * |
| skc_transform_stack_top_weakref(struct skc_transform_stack * const ts) |
| { |
| return ts->weakrefs + ts->count - 1; |
| } |
| |
| // |
| // |
| // |
| |
| void |
| skc_transform_stack_drop(struct skc_transform_stack * const ts) |
| { |
| assert(ts->count >= 1); |
| |
| ts->count -= 1; |
| } |
| |
| void |
| skc_transform_stack_dup(struct skc_transform_stack * const ts) |
| { |
| skc_transform_stack_ensure(ts); |
| |
| union skc_transform_stack_3x3_u * const tos = skc_transform_stack_tos(ts); |
| |
| skc_transform_stack_3x3_u_copy(tos+1,tos); |
| |
| ts->weakrefs[ts->count] = ts->weakrefs[ts->count-1]; |
| |
| ts->count += 1; |
| } |
| |
| // |
| // |
| // |
| |
| void |
| skc_transform_stack_push_matrix(struct skc_transform_stack * const ts, |
| skc_transform_float_t const sx, |
| skc_transform_float_t const shx, |
| skc_transform_float_t const tx, |
| skc_transform_float_t const shy, |
| skc_transform_float_t const sy, |
| skc_transform_float_t const ty, |
| skc_transform_float_t const w0, |
| skc_transform_float_t const w1, |
| skc_transform_float_t const w2) |
| { |
| skc_transform_stack_ensure(ts); |
| |
| union skc_transform_stack_3x3_u * t = ts->transforms + ts->count; |
| |
| t->sx = sx; |
| t->shx = shx; |
| t->tx = tx; |
| |
| t->shy = shy; |
| t->sy = sy; |
| t->ty = ty; |
| |
| t->w0 = w0; |
| t->w1 = w1; |
| t->w2 = w2; |
| |
| ts->weakrefs[ts->count++] = SKC_WEAKREF_INVALID; |
| } |
| |
| void |
| skc_transform_stack_push_identity(struct skc_transform_stack * const ts) |
| { |
| skc_transform_stack_push_matrix(ts, |
| 1.0, 0.0, 0.0, |
| 0.0, 1.0, 0.0, |
| 0.0, 0.0, 1.0); |
| } |
| |
| void |
| skc_transform_stack_push_affine(struct skc_transform_stack * const ts, |
| skc_transform_float_t const sx, |
| skc_transform_float_t const shx, |
| skc_transform_float_t const tx, |
| skc_transform_float_t const shy, |
| skc_transform_float_t const sy, |
| skc_transform_float_t const ty) |
| { |
| skc_transform_stack_push_matrix(ts, |
| sx, shx, tx, |
| shy, sy, ty, |
| 0.0, 0.0, 1.0); |
| } |
| |
| void |
| skc_transform_stack_push_translate(struct skc_transform_stack * const ts, |
| skc_transform_float_t const tx, |
| skc_transform_float_t const ty) |
| { |
| skc_transform_stack_push_matrix(ts, |
| 1.0, 0.0, tx, |
| 0.0, 1.0, ty, |
| 0.0, 0.0, 1.0); |
| } |
| |
| void |
| skc_transform_stack_push_scale(struct skc_transform_stack * const ts, |
| skc_transform_float_t const sx, |
| skc_transform_float_t const sy) |
| { |
| skc_transform_stack_push_matrix(ts, |
| sx, 0.0, 0.0, |
| 0.0, sy, 0.0, |
| 0.0, 0.0, 1.0); |
| } |
| |
| void |
| skc_transform_stack_push_shear(struct skc_transform_stack * const ts, |
| skc_transform_float_t const shx, |
| skc_transform_float_t const shy) |
| { |
| skc_transform_stack_push_matrix(ts, |
| 1.0, shx, 0.0, |
| shy, 1.0, 0.0, |
| 0.0, 0.0, 1.0); |
| } |
| |
| void |
| skc_transform_stack_push_skew_x(struct skc_transform_stack * const ts, |
| skc_transform_float_t const theta) |
| { |
| skc_transform_float_t const tan_theta = SKC_TRANSFORM_TAN(theta); // replace with tanpi if available |
| |
| skc_transform_stack_push_matrix(ts, |
| 1.0, tan_theta,0.0, |
| 0.0, 1.0, 0.0, |
| 0.0, 0.0, 1.0); |
| } |
| |
| void |
| skc_transform_stack_push_skew_y(struct skc_transform_stack * const ts, |
| skc_transform_float_t const theta) |
| { |
| skc_transform_float_t const tan_theta = SKC_TRANSFORM_TAN(theta); // replace with tanpi if available |
| |
| skc_transform_stack_push_matrix(ts, |
| 1.0, 0.0, 0.0, |
| tan_theta, 1.0, 0.0, |
| 0.0, 0.0, 1.0); |
| } |
| |
| void |
| skc_transform_stack_push_rotate(struct skc_transform_stack * const ts, |
| skc_transform_float_t const theta) |
| { |
| skc_transform_float_t const cos_theta = SKC_TRANSFORM_COS(theta); // replace with cospi if available |
| skc_transform_float_t const sin_theta = SKC_TRANSFORM_SIN(theta); // replace with sinpi if available |
| |
| skc_transform_stack_push_matrix(ts, |
| cos_theta,-sin_theta, 0.0, |
| sin_theta, cos_theta, 0.0, |
| 0.0, 0.0, 1.0); |
| } |
| |
| void |
| skc_transform_stack_push_rotate_xy2(struct skc_transform_stack * const ts, |
| skc_transform_float_t const theta, |
| skc_transform_float_t const cx, |
| skc_transform_float_t const cy, |
| skc_transform_float_t const tx, |
| skc_transform_float_t const ty) |
| { |
| skc_transform_float_t const cos_theta = SKC_TRANSFORM_COS(theta); // replace with cospi if available |
| skc_transform_float_t const sin_theta = SKC_TRANSFORM_SIN(theta); // replace with sinpi if available |
| |
| skc_transform_stack_push_matrix(ts, |
| cos_theta,-sin_theta, tx - (cx * cos_theta) + (cy * sin_theta), |
| sin_theta, cos_theta, ty - (cx * sin_theta) - (cy * cos_theta), |
| 0.0, 0.0, 1.0); |
| } |
| |
| void |
| skc_transform_stack_push_rotate_xy(struct skc_transform_stack * const ts, |
| skc_transform_float_t const theta, |
| skc_transform_float_t const cx, |
| skc_transform_float_t const cy) |
| { |
| skc_transform_stack_push_rotate_xy2(ts,theta,cx,cy,cx,cy); |
| } |
| |
| void |
| skc_transform_stack_push_rotate_scale_xy(struct skc_transform_stack * const ts, |
| skc_transform_float_t const theta, |
| skc_transform_float_t const sx, |
| skc_transform_float_t const sy, |
| skc_transform_float_t const cx, |
| skc_transform_float_t const cy) |
| { |
| skc_transform_float_t const cos_theta = SKC_TRANSFORM_COS(theta); // replace with cospi if available |
| skc_transform_float_t const sin_theta = SKC_TRANSFORM_SIN(theta); // replace with sinpi if available |
| |
| skc_transform_stack_push_matrix(ts, |
| sx*cos_theta,-sx*sin_theta, cx - cx*sx*cos_theta + cy*sy*sin_theta, |
| sy*sin_theta, sy*cos_theta, cy - cy*sy*cos_theta - cx*sx*sin_theta, |
| 0.0, 0.0, 1.0); |
| } |
| |
| // |
| // See: https://www.ldv.ei.tum.de/fileadmin/w00bfa/www/content_uploads/Vorlesung_3.2_SpatialTransformations.pdf |
| // |
| |
| #define DET4(m,a,b,c,d) (m->a9[a] * m->a9[d] - m->a9[b] * m->a9[c]) |
| #define DET3(m,a,b,c) (m->a9[a] - m->a9[b] * m->a9[c]) |
| |
| #define DIAG3(m,a,b,c) (m->a9[a] * m->a9[b] * m->a9[c]) |
| #define DIAG2(m,a,b) (m->a9[a] * m->a9[b]) |
| |
| #define X(v,i) v[i*2] |
| #define Y(v,i) v[i*2+1] |
| |
| // |
| // |
| // |
| |
| bool |
| skc_transform_stack_push_quad_to_unit(struct skc_transform_stack * const ts, |
| const float quad_src[8]) |
| { |
| if (!skc_transform_stack_push_unit_to_quad(ts,quad_src)) |
| return false; |
| |
| union skc_transform_stack_3x3_u * const T = skc_transform_stack_tos(ts); |
| |
| #if 0 |
| // |
| // dividing by the determinant (just w2?) |
| // |
| skc_transform_float_t const det = |
| DIAG2(T,0,4) + |
| DIAG3(T,3,7,2) + |
| DIAG3(T,6,1,5) - |
| DIAG3(T,6,4,2) - |
| DIAG2(T,3,1) - |
| DIAG3(T,0,7,5); |
| |
| if (det == 0.0) |
| return false; |
| |
| skc_transform_float_t const rcp = 1.0 / det; |
| |
| const union skc_transform_stack_3x3_u A = |
| { |
| +DET3(T,4,5,7) * rcp, |
| -DET3(T,1,2,7) * rcp, |
| +DET4(T,1,2,4,5) * rcp, |
| |
| -DET3(T,3,5,6) * rcp, |
| +DET3(T,0,2,6) * rcp, |
| -DET4(T,0,2,3,5) * rcp, |
| |
| +DET4(T,3,4,6,7) * rcp, |
| -DET4(T,0,1,6,7) * rcp, |
| +DET4(T,0,1,3,4) * rcp |
| }; |
| |
| #else // just the adjoint can result in large values |
| |
| const union skc_transform_stack_3x3_u A = |
| { |
| +DET3(T,4,5,7), |
| -DET3(T,1,2,7), |
| +DET4(T,1,2,4,5), |
| |
| -DET3(T,3,5,6), |
| +DET3(T,0,2,6), |
| -DET4(T,0,2,3,5), |
| |
| +DET4(T,3,4,6,7), |
| -DET4(T,0,1,6,7), |
| +DET4(T,0,1,3,4) |
| }; |
| |
| #endif |
| |
| skc_transform_stack_3x3_u_copy(T,&A); |
| |
| return true; |
| } |
| |
| // |
| // |
| // |
| |
| bool |
| skc_transform_stack_push_unit_to_quad(struct skc_transform_stack * const ts, |
| const float quad_dst[8]) |
| { |
| skc_transform_float_t const x0 = (skc_transform_float_t)X(quad_dst,0); |
| skc_transform_float_t const y0 = (skc_transform_float_t)Y(quad_dst,0); |
| |
| skc_transform_float_t const x1 = (skc_transform_float_t)X(quad_dst,1); |
| skc_transform_float_t const y1 = (skc_transform_float_t)Y(quad_dst,1); |
| |
| skc_transform_float_t const x2 = (skc_transform_float_t)X(quad_dst,2); |
| skc_transform_float_t const y2 = (skc_transform_float_t)Y(quad_dst,2); |
| |
| skc_transform_float_t const x3 = (skc_transform_float_t)X(quad_dst,3); |
| skc_transform_float_t const y3 = (skc_transform_float_t)Y(quad_dst,3); |
| |
| skc_transform_float_t sx = x1 - x0; |
| skc_transform_float_t shy = y1 - y0; |
| |
| skc_transform_float_t const dx2 = x3 - x2; |
| skc_transform_float_t const dy2 = y3 - y2; |
| |
| skc_transform_float_t const dx3 = -sx - dx2; |
| skc_transform_float_t const dy3 = -shy - dy2; |
| |
| // if both zero then quad_dst is a parallelogram and affine |
| if ((dx3 == 0.0) && (dy3 == 0.0)) |
| { |
| skc_transform_float_t const shx = x2 - x1; |
| skc_transform_float_t const sy = y2 - y1; |
| |
| skc_transform_stack_push_matrix(ts, |
| sx, shx, x0, |
| shy, sy, y0, |
| 0.0, 0.0, 1.0); |
| } |
| else |
| { |
| skc_transform_float_t const dx1 = x1 - x2; |
| skc_transform_float_t const dy1 = y1 - y2; |
| |
| skc_transform_float_t const wx_den = dx1 * dy2 - dx2 * dy1; |
| |
| if (wx_den == 0.0) |
| return false; |
| |
| skc_transform_float_t const w0_num = dx3 * dy2 - dx2 * dy3; |
| skc_transform_float_t const w1_num = dx1 * dy3 - dx3 * dy1; |
| |
| skc_transform_float_t const w0 = w0_num / wx_den; |
| skc_transform_float_t const w1 = w1_num / wx_den; |
| |
| sx += w0 * x1; |
| skc_transform_float_t const shx = x3 - x0 + w1 * x3; |
| |
| shy += w0 * y1; |
| skc_transform_float_t const sy = y3 - y0 + w1 * y3; |
| |
| skc_transform_stack_push_matrix(ts, |
| sx, shx, x0, |
| shy, sy, y0, |
| w0, w1, 1.0); |
| } |
| |
| return true; |
| } |
| |
| // |
| // |
| // |
| |
| bool |
| skc_transform_stack_push_quad_to_quad(struct skc_transform_stack * const ts, |
| const float quad_src[8], |
| const float quad_dst[8]) |
| { |
| if (skc_transform_stack_push_quad_to_unit(ts,quad_src) == false) |
| return false; |
| |
| if (skc_transform_stack_push_unit_to_quad(ts,quad_dst) == false) |
| return false; |
| |
| union skc_transform_stack_3x3_u * const U2Q = skc_transform_stack_tos(ts); |
| union skc_transform_stack_3x3_u * const Q2U = U2Q - 1; |
| union skc_transform_stack_3x3_u Q2Q; |
| |
| // pre-multiply |
| skc_transform_stack_3x3_u_multiply(&Q2Q,U2Q,Q2U); |
| |
| // drop TOS |
| skc_transform_stack_drop(ts); |
| |
| // overwrite TOS |
| skc_transform_stack_3x3_u_copy(Q2U,&Q2Q); |
| |
| return true; |
| } |
| |
| // |
| // |
| // |
| |
| bool |
| skc_transform_stack_push_rect_to_quad(struct skc_transform_stack * const ts, |
| const float x0, |
| const float y0, |
| const float x1, |
| const float y1, |
| const float quad_dst[8]) |
| { |
| if (skc_transform_stack_push_unit_to_quad(ts,quad_dst) == false) |
| return false; |
| |
| const union skc_transform_stack_3x3_u R2U = |
| { |
| SKC_TRANSFORM_RCP((skc_transform_float_t)(x1-x0)), |
| 0.0f, |
| (skc_transform_float_t)-x0, |
| |
| 0.0f, |
| SKC_TRANSFORM_RCP((skc_transform_float_t)(y1-y0)), |
| (skc_transform_float_t)-y0, |
| |
| 0.0f, |
| 0.0f, |
| 1.0, |
| }; |
| |
| union skc_transform_stack_3x3_u * const U2Q = skc_transform_stack_tos(ts); |
| union skc_transform_stack_3x3_u Q2Q; |
| |
| // pre-multiply |
| skc_transform_stack_3x3_u_multiply(&Q2Q,U2Q,&R2U); |
| |
| // overwrite TOS |
| skc_transform_stack_3x3_u_copy(U2Q,&Q2Q); |
| |
| return true; |
| } |
| |
| // |
| // |
| // |
| |
| void |
| skc_transform_stack_concat(struct skc_transform_stack * const ts) |
| { |
| // |
| // Transformation stack matrices are _post-multiplied_ and the top |
| // of stack is replaced with the product: |
| // |
| // TOS' = TOS[-1] * TOS |
| // |
| // -or- |
| // |
| // C = A * B |
| // B = C |
| // |
| if (ts->count <= 1) |
| return; |
| |
| // get A and B |
| union skc_transform_stack_3x3_u * const B = skc_transform_stack_tos(ts); |
| const union skc_transform_stack_3x3_u * const A = B - 1; |
| |
| union skc_transform_stack_3x3_u C; |
| |
| // post multiply |
| skc_transform_stack_3x3_u_multiply(&C,A,B); |
| |
| // overwrite TOS |
| skc_transform_stack_3x3_u_copy(B,&C); |
| |
| // invalidate TOS |
| *skc_transform_stack_top_weakref(ts) = SKC_WEAKREF_INVALID; |
| } |
| |
| // |
| // |
| // |
| |
| void |
| skc_transform_stack_transform_affine(struct skc_transform_stack * const ts, |
| skc_transform_float_t const x_pre, |
| skc_transform_float_t const y_pre, |
| skc_transform_float_t * const x_post, |
| skc_transform_float_t * const y_post) |
| { |
| union skc_transform_stack_3x3_u const * const t = skc_transform_stack_tos(ts); |
| |
| *x_post = x_pre * t->sx + y_pre * t->shx + t->tx; |
| *y_post = x_pre * t->shy + y_pre * t->sy + t->ty; |
| } |
| |
| // |
| // |
| // |