/******************************************************************************
Online C Compiler.
Code, Compile, Run and Debug C program online.
Write your code in this editor and press "Run" button to compile and execute it.
*******************************************************************************/
#include <string.h> /* For memcpy, memset etc */
#include <stdio.h> /* For printf */
#include <stdlib.h> /* Standard library */
#include <math.h> /* For sqrtf */
#include <float.h> /* Required for FLT_EPSILON, FLT_MAX, FLT_MIN */
#include <stddef.h> /* Requried for size_t, NULL etc... */
#include <time.h> /* For srand, clock */
#include <stdarg.h> /* For ... arguments */
void print(const float A[], const size_t row, const size_t column) {
size_t i, j;
for (i = 0; i < row; i++) {
for (j = 0; j < column; j++) {
printf("%0.7f\t", *(A++));
}
printf("\n");
}
printf("\n");
}
void mul(const float A[], const float B[], float C[], const size_t row_a, const size_t column_a, const size_t column_b){
/* Decleration */
size_t i, j, k;
/* Data matrix */
const float* data_a, * data_b;
for (i = 0; i < row_a; i++) {
/* Then we go through every column of b */
for (j = 0; j < column_b; j++) {
data_a = A;
data_b = &B[j];
C[0] = 0.0f; /* Reset */
/* And we multiply rows from a with columns of b */
for (k = 0; k < column_a; k++) {
*C += data_a[0] * data_b[0];
data_a++;
data_b += column_b;
}
C++; /* ;) */
}
A += column_a;
}
}
/*
* Fourth-order Runge–Kutta method.
* Runge-Kutta method is not the best ODE solver, but it's the best ODE solver for fixed step time iteration.
* h - Step time
* Y[iterations*N] - Output
* y[N] - Initial state vector
* N - Dimension for y-vector
* odefun(const float t, float y[], const float* matrices[], const size_t rows[], const size_t columns[])
* ... = const float matrixA[], const size_t rowA, const size_t columnA, const float matrixB[], const size_t rowB, const size_t columnB, const float matrixC[], const size_t rowC, const size_t columnC, etc...
*/
void rk4args(const size_t iterations, const float h, float Y[], float y[], const size_t N, void (*odefun)(float, float*, float**, const size_t*, const size_t*), const size_t number_of_pointers, ...) {
/* Variables */
size_t i, j;
/* Constants */
const size_t length = N * sizeof(float);
/* Create vectors */
float* k1 = (float*)malloc(length);
float* k2 = (float*)malloc(length);
float* k3 = (float*)malloc(length);
float* k4 = (float*)malloc(length);
/* Temporary vectors */
float* yt = (float*)malloc(length);
/* Variables */
float t = 0.0f;
/* Initial output */
memcpy(Y, y, length);
/* Variable arguments */
va_list args;
va_start(args, number_of_pointers);
/* Pointers */
float** matrices = (float**)malloc(number_of_pointers * sizeof(float*));
size_t* rows = (size_t*)malloc(number_of_pointers * sizeof(size_t));
size_t* columns = (size_t*)malloc(number_of_pointers * sizeof(size_t));
/* Get the arguments */
for (i = 0; i < number_of_pointers; i++) {
matrices[i] = va_arg(args, float*);
rows[i] = va_arg(args, const size_t);
columns[i] = va_arg(args, const size_t);
}
/* Perform Runge-Kutta */
for (i = 1; i < iterations; i++) {
/* Receive old output */
for (j = 0; j < N; j++) {
y[j] = Y[(i - 1) * N + j];
}
/* First statement */
memcpy(yt, y, length);
odefun(t, yt, matrices, rows, columns);
for (j = 0; j < N; j++) {
k1[j] = yt[j] * h;
}
/* Second statement */
memcpy(yt, y, length);
for (j = 0; j < N; j++) {
yt[j] += k1[j] / 2.0f;
}
odefun(t + h / 2.0f, yt, matrices, rows, columns);
for (j = 0; j < N; j++) {
k2[j] = yt[j] * h;
}
/* Third statement */
memcpy(yt, y, length);
for (j = 0; j < N; j++) {
yt[j] += k2[j] / 2.0f;
}
odefun(t + h / 2.0f, yt, matrices, rows, columns);
for (j = 0; j < N; j++) {
k3[j] = yt[j] * h;
}
/* Fourth statement */
memcpy(yt, y, length);
for (j = 0; j < N; j++) {
yt[j] += k3[j];
}
odefun(t + h, yt, matrices, rows, columns);
for (j = 0; j < N; j++) {
k4[j] = yt[j] * h;
}
/* Save output */
for (j = 0; j < N; j++) {
Y[i * N + j] = y[j] + (k1[j] + 2.0f * k2[j] + 2.0f * k3[j] + k4[j]) / 6.0f;
}
/* Update t */
t += h;
}
/* Free */
free(k1);
free(k2);
free(k3);
free(k4);
free(yt);
free(rows);
free(columns);
free(matrices);
/* Close */
va_end(args);
}
/* Constants */
#define N 2
#define h 0.1f
#define row_a 2
#define column_a 2
#define row_b 2
#define column_b 1
#define row_u 1
#define column_u 1
#define ITERATIONS 200
/* Create ODE equation */
void odefun(const float t, float y[], float* matrices[], const size_t rows[], const size_t columns[]) {
/* Get the matrices */
float* A = matrices[0];
float* B = matrices[1];
float* u = matrices[2];
/* Get the sizes */
size_t row_A = rows[0];
size_t column_A = columns[0];
size_t row_B = rows[1];
size_t column_B = columns[1];
size_t row_U = rows[2];
size_t column_U = columns[2];
/* Solve y = A*y + B*u */
float Ay[2];
float Bu[2];
mul(A, y, Ay, row_A, column_A, 1);
mul(B, u, Bu, row_B, column_B, 1);
size_t i;
for (i = 0; i < row_A; i++) {
y[i] = Ay[i] + Bu[i];
}
}
int main() {
clock_t start, end;
float cpu_time_used;
start = clock();
/* Declare intial state */
float y0[N] = { 0 };
/* Declare output */
float Y[ITERATIONS * N];
/* Declare number of pointers */
const size_t number_of_pointers = 3;
const float k = 5.0f; /* Spring constant [N/m] */
const float b = 1.4f; /* Damper constant [Ns/m] */
const float m = 2.0f; /* Mass [kg] */
const float F = 8.0f; /* Force [N] */
const float A[row_a * column_a] = { 0, 1, -k / m, -b / m };
const float B[row_b * column_b] = { 0, 1 / m };
const float u[row_u * column_u] = { F };
/* Perform Runge-Kutta 4:th order with extended arguments ... of const float[], const size_t, const size_t etc... */
rk4args(ITERATIONS, h, Y, y0, N, odefun, number_of_pointers, A, (const size_t)row_a, (const size_t)column_a, B, (const size_t)row_b, (const size_t)column_b, u, (const size_t)row_u, (const size_t)column_u);
end = clock();
cpu_time_used = ((float)(end - start)) / CLOCKS_PER_SEC;
printf("\nTotal speed was %f\n", cpu_time_used);
/* Plot */
print(Y, ITERATIONS, N);
return EXIT_SUCCESS;
}
