1:#include <math.h>
2:#include "matematicas.h"
3:
4:/******************************************************************************/
5:/* Devuelve el vector de posición de un punto */
6:/* Parametros --> Punto3D p: el punto a partir del que se construye el vector */
7:/* Salida ------> El vector */
8:/******************************************************************************/
9:Vector3D vectorPosicion(Punto3D p) {
10: Vector3D res;
11:
12: res.a = p.x;
13: res.b = p.y;
14: res.c = p.z;
15:
16: return (res);
17:}
18:
19:/******************************************************************************/
20:/* Calcula un vector desde un punto inicial a uno final */
21:/* Parametros --> Punto3D p: el punto inicial */
22:/* Punto3D q: el punto final */
23:/* Salida ------> El vector entre los puntos */
24:/******************************************************************************/
25:Vector3D vector(Punto3D p, Punto3D q) {
26: Vector3D res;
27:
28: res.a = q.x-p.x;
29: res.b = q.y-p.y;
30: res.c = q.z-p.z;
31: return (res);
32:}
33:
34:/******************************************************************************/
35:/* Actualiza las coordenadas de un punto */
36:/* Parametros --> float x: la nueva coordenada x del punto */
37:/* float y: idem para la y */
38:/* float z: idem para la z */
39:/* Salida ------> El nuevo punto actualizado */
40:/******************************************************************************/
41:Punto3D actualizaPunto3D(float x, float y, float z) {
42: Punto3D p;
43:
44: p.x = x;
45: p.y = y;
46: p.z = z;
47:
48: return (p);
49:}
50:
51:/******************************************************************************/
52:/* Actualiza las componentes de un vector */
53:/* Parametros --> float a: la nueva componente a del punto */
54:/* float b: idem para la b */
55:/* float c: idem para la c */
56:/* Salida ------> El nuevo vector actualizado */
57:/******************************************************************************/
58:Vector3D actualizaVector3D(float a, float b, float c) {
59: Vector3D v;
60:
61: v.a = a;
62: v.b = b;
63: v.c = c;
64:
65: return (v);
66:}
67:
68:/******************************************************************************/
69:/* Producto escalar de dos vectores */
70:/* Parametros --> Vector3D v: uno de los vectores */
71:/* Vector3D w: el otro vector */
72:/* Salida ------> El valor del producto escalar */
73:/******************************************************************************/
74:float productoEscalar(Vector3D v, Vector3D w) {
75: float resultado = 0.0f;
76:
77: resultado = (v.a*w.a)+(v.b*w.b)+(v.c*w.c);
78:
79: return (resultado);
80:}
81:
82:/******************************************************************************/
83:/* Producto vectorial de dos vectores */
84:/* Parametros --> Vector3D v: uno de los vectores */
85:/* Vector3D w: el otro vector */
86:/* Salida ------> El vector resultante */
87:/******************************************************************************/
88:Vector3D productoVectorial(Vector3D v, Vector3D w) {
89: float longitud;
90: Vector3D res;
91:
92: res.a = v.b*w.c-(v.c*w.b);
93: res.b = v.c*w.a-(v.a*w.c);
94: res.c = v.a*w.b-(v.b*w.a);
95: longitud = (float)sqrt((res.a*res.a)+(res.b*res.b)+(res.c*res.c));
96: if(longitud != 0) {
97: res.a /= longitud;
98: res.b /= longitud;
99: res.c /= longitud;
100: }
101:
102: return (res);
103:}
104:
105:/******************************************************************************/
106:/* Normaliza un vector */
107:/* Parametros --> Vector3D v: el vector que se va a normalizar */
108:/* Salida ------> El vector normalizado */
109:/******************************************************************************/
110:Vector3D normaliza(Vector3D v) {
111: float longitud = (float)sqrt(productoEscalar(v, v));
112:
113: if(longitud != 0) {
114: v.a /= longitud;
115: v.b /= longitud;
116: v.c /= longitud;
117: }
118:
119: return (v);
120:}
121:
122:/******************************************************************************/
123:/* Comprueba si dos puntos son distintos */
124:/* Parametros --> Punto3D p: uno de los puntos */
125:/* Punto3D q: el otro */
126:/* Salida ------> DISTINTOS o IGUALES */
127:/******************************************************************************/
128:int distintos(Punto3D p, Punto3D q) {
129: int distintos = IGUALES;
130:
131: if(p.x != q.x || p.y != q.y || p.z != q.z) distintos = DISTINTOS;
132:
133: return distintos;
134:}
135:
136:/******************************************************************************/
137:/* Calcula la matriz modelo-vista a partir de dos puntos de agarre y el */
138:/* radio de la esfera virtual */
139:/* Parametros --> Punto3D pInicio: punto inicial de agarre. */
140:/* Punto3D pFin: punto final de agarre. */
141:/* matriz[]: la matriz que se calcula */
142:/* float R2: el cuadrado del radio de la esfera virtual */
143:/* Salida ------> Ninguna */
144:/******************************************************************************/
145:void calculaMatrizModeloVista(Punto3D pInicio, Punto3D pFin, float matriz[],
146: float R2) {
147: float z = (float)sqrt(R2-((pInicio.x*pInicio.x)+(pInicio.y*pInicio.y)));
148: Vector3D vInicio = actualizaVector3D(pInicio.x, pInicio.y, z);
149: Vector3D vFin, normal;
150: float coseno;
151: float angulo;
152:
153: vInicio = normaliza(vInicio);
154: z = (float)sqrt(R2-((pFin.x*pFin.x)+(pFin.y*pFin.y)));
155: vFin = actualizaVector3D(pFin.x, pFin.y, z);
156: vFin = normaliza(vFin);
157: normal = productoVectorial(vFin, vInicio);
158: normal = normaliza(normal);
159: coseno = productoEscalar(vInicio, vFin);
160: if(coseno > 1.0f) coseno = 1.0f;
161: else if(coseno < -1.0f) coseno = -1.0f;
162: angulo = (float)acos(coseno);
163: angulo *= 5.0f;
164: coseno = (float)cos(angulo);
165: matrizRotacion(normal.a, normal.b, normal.c, coseno, matriz);
166:}
167:
168:/******************************************************************************/
169:/* Calcula la matriz de rotacion en torno a un eje cualquiera */
170:/* Parametros --> float rx, ry, rz: el eje de rotacion */
171:/* float coseno: el coseno de rotacion */
172:/* float matriz[]: la matriz donde se devuelve la rotacion */
173:/* Salida ------> Ninguna */
174:/******************************************************************************/
175:void matrizRotacion(float rx, float ry, float rz, float coseno, float matriz[]) {
176: float seno = (float)sqrt(1.0f-(coseno*coseno));
177: float vers = 1.0f - coseno;
178:
179: matriz[0] = (rx*rx*vers) + coseno;
180: matriz[1] = (rx*ry*vers) + (rz*seno);
181: matriz[2] = (rx*rz*vers) - (ry*seno);
182: matriz[4] = (rx*ry*vers) - (rz*seno);
183: matriz[5] = (ry*ry*vers) + (coseno);
184: matriz[6] = (ry*rz*vers) + (rx*seno);
185: matriz[8] = (rx*rz*vers) + (ry*seno);
186: matriz[9] = (ry*rz*vers) - (rx*seno);
187: matriz[10] = (rz*rz*vers) + (coseno);
188: matriz[3] = matriz[7] = matriz[11] = 0.0f;
189: matriz[12] = matriz[13] = 0.0f; matriz[14] = 0.0f;
190: matriz[15] = 1.0f;
191:}
192:
193:/******************************************************************************/
194:/* Multiplica dos matrices de tamaño 4x4 */
195:/* Parametros --> float m1[]: una de las matrices. */
196:/* float m2[]: la otra. */
197:/* floar resultado[]=m1[]*m2[]. */
198:/******************************************************************************/
199:void multiplicaMatrices(float m1[], float m2[], float resultado[]) {
200: float longitud;
201:
202: resultado[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2] + m1[12]*m2[3];
203: resultado[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6] + m1[12]*m2[7];
204: resultado[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10] + m1[12]*m2[11];
205: resultado[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12]*m2[15];
206: resultado[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2] + m1[13]*m2[3];
207: resultado[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6] + m1[13]*m2[7];
208: resultado[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10] + m1[13]*m2[11];
209: resultado[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13]*m2[15];
210: resultado[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2] + m1[14]*m2[3];
211: resultado[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6] + m1[14]*m2[7];
212: resultado[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10] + m1[14]*m2[11];
213: resultado[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14]*m2[15];
214: resultado[3] = 0.0f; resultado[7] = 0.0f; resultado[11] = 0.0f;
215: resultado[15] = 1.0f;
216:
217: longitud = (float)sqrt(resultado[0]*resultado[0] + resultado[1]*resultado[1] + resultado[2]*resultado[2]);
218: resultado[0] /= longitud;
219: resultado[1] /= longitud;
220: resultado[2] /= longitud;
221:
222: longitud = (float)sqrt(resultado[4]*resultado[4] + resultado[5]*resultado[5] + resultado[6]*resultado[6]);
223: resultado[4] /= longitud;
224: resultado[5] /= longitud;
225: resultado[6] /= longitud;
226:
227: longitud = (float)sqrt(resultado[8]*resultado[8] + resultado[9]*resultado[9] + resultado[10]*resultado[10]);
228: resultado[8] /= longitud;
229: resultado[9] /= longitud;
230: resultado[10] /= longitud;
231:}
232:
233:/******************************************************************************/
234:/* Copia una matriz sobre otra. */
235:/* Parametros --> float m1[]: matriz destino. */
236:/* float m2[]: matriz origen. */
237:/* Salida ------> Ninguna. */
238:/******************************************************************************/
239:void asigna(float m1[], float m2[]) {
240: int i;
241:
242: for(i = 0; i < 16; i++) m1[i] = m2[i];
243:}
244:
245:/******************************************************************************/
246:/* Multiplica una matriz por un vector. */
247:/* Parametros --> float producto[]: la matriz que multiplica */
248:/* float Vector3D v: el vector que es multiplicado */
249:/* Salida ------> Vector3D: el resultado de la multiplicacion */
250:/******************************************************************************/
251:Vector3D multiplicaMatrizVector(float producto[], Vector3D v) {
252: Vector3D resultado;
253:
254: resultado.a = producto[0] * v.a + producto[4] * v.b + producto[8] * v.c;
255: resultado.b = producto[1] * v.a + producto[5] * v.b + producto[9] * v.c;
256: resultado.c = producto[2] * v.a + producto[6] * v.b + producto[10] * v.c;
257:
258: return (resultado);
259:}