Project Ne10
An Open Optimized Software Library Project for the ARM Architecture
NE10_physics.c
1 /*
2  * Copyright 2014-15 ARM Limited and Contributors.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of ARM Limited nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY ARM LIMITED AND CONTRIBUTORS "AS IS" AND
17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL ARM LIMITED AND CONTRIBUTORS BE LIABLE FOR ANY
20  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  */
27 
28 /*
29  * NE10 Library : physics/NE10_physics.c
30  */
31 
32 #include "NE10_types.h"
33 
47 static inline ne10_vec2f_t ne10_mul_matvec_float (ne10_mat2x2f_t T, ne10_vec2f_t v)
48 {
49  ne10_vec2f_t tmp;
50  ne10_float32_t x = (T.c2.r2 * v.x - T.c2.r1 * v.y) + T.c1.r1;
51  ne10_float32_t y = (T.c2.r1 * v.x + T.c2.r2 * v.y) + T.c1.r2;
52  tmp.x = x;
53  tmp.y = y;
54  return tmp;
55 }
56 
57 static inline ne10_float32_t min (float a, ne10_float32_t b)
58 {
59  return a < b ? a : b;
60 }
61 
62 static inline ne10_vec2f_t min_2f (ne10_vec2f_t a, ne10_vec2f_t b)
63 {
64  ne10_vec2f_t tmp = {min (a.x, b.x), min (a.y, b.y) };
65  return tmp;
66 }
67 
68 static inline ne10_float32_t max (float a, ne10_float32_t b)
69 {
70  return a > b ? a : b;
71 }
72 
73 static inline ne10_vec2f_t max_2f (ne10_vec2f_t a, ne10_vec2f_t b)
74 {
75  ne10_vec2f_t tmp = {max (a.x, b.x), max (a.y, b.y) };
76  return tmp;
77 }
78 
95 void ne10_physics_compute_aabb_vec2f_c (ne10_mat2x2f_t *aabb,
96  ne10_vec2f_t *vertices,
97  ne10_mat2x2f_t *xf,
98  ne10_vec2f_t *radius,
99  ne10_uint32_t vertex_count)
100 {
101  ne10_vec2f_t lower = ne10_mul_matvec_float (*xf, vertices[0]);
102  ne10_vec2f_t upper = lower;
103  ne10_vec2f_t v;
104  ne10_int32_t i;
105 
106  for (i = 1; i < vertex_count; ++i)
107  {
108  v = ne10_mul_matvec_float (*xf, vertices[i]);
109  lower = min_2f (lower, v);
110  upper = max_2f (upper, v);
111  }
112 
113  aabb->c1.r1 = lower.x - radius->x;
114  aabb->c1.r2 = lower.y - radius->y;
115  aabb->c2.r1 = upper.x + radius->x;
116  aabb->c2.r2 = upper.y + radius->y;
117 
118 }
119 
133  ne10_vec3f_t *v_wa,
134  ne10_vec2f_t *ra,
135  ne10_vec3f_t *v_wb,
136  ne10_vec2f_t *rb,
137  ne10_uint32_t count)
138 {
139  ne10_int32_t i;
140  ne10_vec2f_t va;
141  ne10_vec2f_t vb;
142 
143  for (i = 0; i < count; i++)
144  {
145  va.x = v_wa->x - v_wa->z * ra->y;
146  va.y = v_wa->y + v_wa->z * ra->x;
147  vb.x = v_wb->x - v_wb->z * rb->y;
148  vb.y = v_wb->y + v_wb->z * rb->x;
149 
150  dv->x = vb.x - va.x;
151  dv->y = vb.y - va.y;
152 
153  v_wa++;
154  v_wb++;
155  ra++;
156  rb++;
157  dv++;
158 
159  }
160 }
161 
177  ne10_vec3f_t *v_wb,
178  ne10_vec2f_t *ra,
179  ne10_vec2f_t *rb,
180  ne10_vec2f_t *ima,
181  ne10_vec2f_t *imb,
182  ne10_vec2f_t *p,
183  ne10_uint32_t count)
184 {
185  ne10_int32_t i;
186 
187  for (i = 0; i < count; i++)
188  {
189  v_wa->x -= ima->x * p->x;
190  v_wa->y -= ima->x * p->y;
191  v_wa->z -= ima->y * (ra->x * p->y - ra->y * p->x);
192 
193  v_wb->x += imb->x * p->x;
194  v_wb->y += imb->x * p->y;
195  v_wb->z += imb->y * (rb->x * p->y - rb->y * p->x);
196 
197  v_wa++;
198  v_wb++;
199  ra++;
200  rb++;
201  ima++;
202  imb++;
203  p++;
204  }
205 }
ne10_physics_relative_v_vec2f_c
void ne10_physics_relative_v_vec2f_c(ne10_vec2f_t *dv, ne10_vec3f_t *v_wa, ne10_vec2f_t *ra, ne10_vec3f_t *v_wb, ne10_vec2f_t *rb, ne10_uint32_t count)
calculate relative velocity at contact.
Definition: NE10_physics.c:132
ne10_vec3f_t
a 3-tuple of ne10_float32_t values.
Definition: NE10_types.h:96
ne10_physics_compute_aabb_vec2f_c
void ne10_physics_compute_aabb_vec2f_c(ne10_mat2x2f_t *aabb, ne10_vec2f_t *vertices, ne10_mat2x2f_t *xf, ne10_vec2f_t *radius, ne10_uint32_t vertex_count)
compute AABB for ploygon.
Definition: NE10_physics.c:95
ne10_physics_apply_impulse_vec2f_c
void ne10_physics_apply_impulse_vec2f_c(ne10_vec3f_t *v_wa, ne10_vec3f_t *v_wb, ne10_vec2f_t *ra, ne10_vec2f_t *rb, ne10_vec2f_t *ima, ne10_vec2f_t *imb, ne10_vec2f_t *p, ne10_uint32_t count)
apply contact impulse.
Definition: NE10_physics.c:176
ne10_vec2f_t
a 2-tuple of ne10_float32_t values.
Definition: NE10_types.h:87