void
fix16_vector3_zero(fix16_vector3_t *result)
{
result->x = F16(0.0f);
result->y = F16(0.0f);
result->z = F16(0.0f);
}
void BackGround::render() {
float f = getBlendFactor();
GD.RestoreContext();
GD.ClearColorRGB(blend(BLACK, SKY, f));
//GD.ClearColorA(255*f);
//GD.ClearColorRGB(255*f, 255*f, 255*f);
//GD.ClearColorRGB(SKY);
GD.SaveContext();
GD.Clear();
GD.Begin(BITMAPS);
GD.BitmapHandle(RANDOMDOTS_HANDLE);
GD.Cell(RANDOMDOTS_CELLS - 1);
GD.Cell(RANDOMDOTS_CELLS - 1);
//GD.BitmapSize(BILINEAR,REPEAT,REPEAT,(2*64+SCREEN_WIDTH)/2,(2*64+SCREEN_HEIGHT)/2);
GD.BitmapSize(BILINEAR, REPEAT, REPEAT, (SCREEN_WIDTH), (SCREEN_HEIGHT));
GD.cmd_loadidentity();
GD.cmd_translate(F16(480 * 1.41 / 2 - position.x),
F16(272 * 1.41 / 2 + position.y));
GD.cmd_rotate(3333);
GD.cmd_translate(F16(-480 / 2), F16(-272 / 2));
GD.cmd_setmatrix();
GD.ColorA(255 * (clamp(f, 0.3, 1)));
GD.Vertex2f(0, 0);
GD.RestoreContext();
update();
}
void kalman_gravity_demo_lambda()
{
// initialize the filter
kalman_gravity_init();
mf16 *x = kalman_get_state_vector(&kf);
mf16 *z = kalman_get_observation_vector(&kfm);
// forcibly increase uncertainty in every prediction step by ~20% (1/lambda^2)
const fix16_t lambda = F16(0.9);
// filter!
uint_fast16_t i;
for (i = 0; i < MEAS_COUNT; ++i)
{
// prediction.
kalman_predict_tuned(&kf, lambda);
// measure ...
fix16_t measurement = fix16_add(real_distance[i], measurement_error[i]);
matrix_set(z, 0, 0, measurement);
// update
kalman_correct(&kf, &kfm);
}
// fetch estimated g
const fix16_t g_estimated = x->data[2][0];
const float value = fix16_to_float(g_estimated);
assert(value > 9.7 && value < 10);
}
void
fix16_vector3_normalized(const fix16_vector3_t *v0, fix16_vector3_t *result)
{
fix16_t inv_length;
inv_length = fix16_div(F16(1.0f), fix16_vector3_length(v0));
fix16_vector3_scaled(inv_length, v0, result);
}
void
fix16_vector3_normalize(fix16_vector3_t *result)
{
fix16_t inv_length;
inv_length = fix16_div(F16(1.0f), fix16_vector3_length(result));
fix16_vector3_scale(inv_length, result);
}
void Piccolo80bitslice16_core(const u8* message, const u8* subkeys, u8* ciphertext){
word s[(BITSLICE16_P/2)], t0, t1, t2, t3, t4;
int i;
/* load state */
for(i = 0; i < BITSLICE16_P/2; i++){
s[i] = LOAD(((u64*)message) + 2*i);
inverse_bytes_endian64(s[i]);
}
/* pack state */
Piccolo_packing16(s[0], s[1], s[2], s[3], s[4], s[5], s[6], s[7], t0, t1, t2);
/* the encryption process */
int r;
AddKey(s[4], s[5], s[6], s[7], Piccolo80_wk01(subkeys)[0], Piccolo80_wk01(subkeys)[1], Piccolo80_wk01(subkeys)[2], Piccolo80_wk01(subkeys)[3]);
for(r = 0; r < 25; r++)
{
F16(s[4], s[5], s[6], s[7], s[0], s[1], s[2], s[3], t0, t1, t2, t3, t4);
switch(r%5){
case 0:
case 2: AddKey(s[0], s[1], s[2], s[3], Piccolo80_rk23(subkeys)[0], Piccolo80_rk23(subkeys)[1], Piccolo80_rk23(subkeys)[2], Piccolo80_rk23(subkeys)[3]);
break;
case 1:
case 4: AddKey(s[0], s[1], s[2], s[3], Piccolo80_rk01(subkeys)[0], Piccolo80_rk01(subkeys)[1], Piccolo80_rk01(subkeys)[2], Piccolo80_rk01(subkeys)[3]);
break;
case 3: AddKey(s[0], s[1], s[2], s[3], Piccolo80_rk44(subkeys)[0], Piccolo80_rk44(subkeys)[1], Piccolo80_rk44(subkeys)[2], Piccolo80_rk44(subkeys)[3]);
break;
}
AddKey(s[0], s[1], s[2], s[3], Piccolo80_constants[r][0], Piccolo80_constants[r][1], Piccolo80_constants[r][2], Piccolo80_constants[r][3]);
if(r < 24){
Piccolo_RoundPermutation(s[0], s[1], s[2], s[3], s[4], s[5], s[6], s[7]);
}
}
AddKey(s[4], s[5], s[6], s[7], Piccolo80_wk23(subkeys)[0], Piccolo80_wk23(subkeys)[1], Piccolo80_wk23(subkeys)[2], Piccolo80_wk23(subkeys)[3]);
/* unpack state */
Piccolo_unpacking16(s[0], s[1], s[2], s[3], s[4], s[5], s[6], s[7], t0, t1, t2);
/* store the results back to ciphertext */
for(i = 0; i < (BITSLICE16_P/2); i++){
inverse_bytes_endian64(s[i]);
STORE(s[i], ((u64*)ciphertext) + 2*i);
}
return;
}
void AnimationTemplate::renderFrame(const float x, const float y,
const float angle, const float scale, unsigned short frame) {
GD.BitmapHandle(id);
GD.Cell(startCell + frame);
GD.BitmapSize(BILINEAR, BORDER, BORDER, scale * width * SQRT2,
scale * height * SQRT2);
GD.cmd_loadidentity();
GD.cmd_translate(F16(scale * width * SQRT2 / 2),
F16(scale * height * SQRT2 / 2));
GD.cmd_rotate(FURMANS(angle));
GD.cmd_translate(F16(-scale * width / 2), F16(-scale * height / 2));
GD.cmd_scale(F16(scale), F16(scale));
GD.cmd_setmatrix();
cam.Vertex2f(x - offsetX * scale, y + offsetY * scale);
}
void Piccolo128bitslice16_core(const u8* message, const u8* subkeys, u8* ciphertext){
int i;
word s[(BITSLICE16_P/2)], t0, t1, t2, t3, t4;
/* load state */
for(i = 0; i < (BITSLICE16_P/2); i++){
s[i] = LOAD(((u64*)message) + 2*i);
inverse_bytes_endian64(s[i]);
}
/* pack state */
Piccolo_packing16(s[0], s[1], s[2], s[3], s[4], s[5], s[6], s[7], t0, t1, t2);
/* the encryption process */
int r;
AddKey(s[4], s[5], s[6], s[7], Piccolo128_wk01(subkeys)[0], Piccolo128_wk01(subkeys)[1], Piccolo128_wk01(subkeys)[2], Piccolo128_wk01(subkeys)[3]);
for(r = 0; r < 31; r++)
{
F16(s[4], s[5], s[6], s[7], s[0], s[1], s[2], s[3], t0, t1, t2, t3, t4);
int keyc0 = Piccolo128_key_constants0[r];
int keyc1 = Piccolo128_key_constants1[r];
AddKey(s[0], s[1], s[2], s[3], Piccolo128_rk(subkeys,keyc0)[0], Piccolo128_rk(subkeys,keyc0)[1], Piccolo128_rk(subkeys,keyc0)[2], Piccolo128_rk(subkeys,keyc0)[3]);
AddKey(s[0], s[1], s[2], s[3], Piccolo128_rk(subkeys,keyc1)[0], Piccolo128_rk(subkeys,keyc1)[1], Piccolo128_rk(subkeys,keyc1)[2], Piccolo128_rk(subkeys,keyc1)[3]);
AddKey(s[0], s[1], s[2], s[3], Piccolo128_constants[r][0], Piccolo128_constants[r][1], Piccolo128_constants[r][2], Piccolo128_constants[r][3]);
if(r < 30){
Piccolo_RoundPermutation(s[0], s[1], s[2], s[3], s[4], s[5], s[6], s[7]);
}
}
AddKey(s[4], s[5], s[6], s[7], Piccolo128_wk23(subkeys)[0], Piccolo128_wk23(subkeys)[1], Piccolo128_wk23(subkeys)[2], Piccolo128_wk23(subkeys)[3]);
/* unpack state */
Piccolo_unpacking16(s[0], s[1], s[2], s[3], s[4], s[5], s[6], s[7], t0, t1, t2);
/* store the results back to ciphertext */
for(i = 0; i < (BITSLICE16_P/2); i++){
inverse_bytes_endian64(s[i]);
STORE(s[i], ((u64*)ciphertext) + 2*i);
}
return;
}
/*!
* \brief Initializes the gravity Kalman filter
*/
static void kalman_gravity_init()
{
/************************************************************************/
/* initialize the filter structures */
/************************************************************************/
#if USE_UNCONTROLLED
kalman_filter_initialize_uc(&kf, KALMAN_NUM_STATES);
#else
kalman_filter_initialize(&kf, KALMAN_NUM_STATES, KALMAN_NUM_INPUTS);
#endif
kalman_observation_initialize(&kfm, KALMAN_NUM_STATES, KALMAN_NUM_MEASUREMENTS);
/************************************************************************/
/* set initial state */
/************************************************************************/
#if USE_UNCONTROLLED
mf16 *x = kalman_get_state_vector_uc(&kf);
#else
mf16 *x = kalman_get_state_vector(&kf);
#endif
x->data[0][0] = 0; // s_i
x->data[1][0] = 0; // v_i
x->data[2][0] = fix16_from_float(6); // g_i
/************************************************************************/
/* set state transition */
/************************************************************************/
#if USE_UNCONTROLLED
mf16 *A = kalman_get_state_transition_uc(&kf);
#else
mf16 *A = kalman_get_state_transition(&kf);
#endif
// set time constant
const fix16_t T = fix16_one;
const fix16_t Tsquare = fix16_sq(T);
// helper
const fix16_t fix16_half = fix16_from_float(0.5);
// transition of x to s
matrix_set(A, 0, 0, fix16_one); // 1
matrix_set(A, 0, 1, T); // T
matrix_set(A, 0, 2, fix16_mul(fix16_half, Tsquare)); // 0.5 * T^2
// transition of x to v
matrix_set(A, 1, 0, 0); // 0
matrix_set(A, 1, 1, fix16_one); // 1
matrix_set(A, 1, 2, T); // T
// transition of x to g
matrix_set(A, 2, 0, 0); // 0
matrix_set(A, 2, 1, 0); // 0
matrix_set(A, 2, 2, fix16_one); // 1
/************************************************************************/
/* set covariance */
/************************************************************************/
#if USE_UNCONTROLLED
mf16 *P = kalman_get_system_covariance_uc(&kf);
#else
mf16 *P = kalman_get_system_covariance(&kf);
#endif
matrix_set_symmetric(P, 0, 0, fix16_half); // var(s)
matrix_set_symmetric(P, 0, 1, 0); // cov(s,v)
matrix_set_symmetric(P, 0, 2, 0); // cov(s,g)
matrix_set_symmetric(P, 1, 1, fix16_one); // var(v)
matrix_set_symmetric(P, 1, 2, 0); // cov(v,g)
matrix_set_symmetric(P, 2, 2, fix16_one); // var(g)
/************************************************************************/
/* set system process noise */
/************************************************************************/
#if USE_UNCONTROLLED
mf16 *Q = kalman_get_system_process_noise_uc(&kf);
mf16_fill(Q, F16(0.0001));
#endif
/************************************************************************/
/* set measurement transformation */
/************************************************************************/
mf16 *H = kalman_get_observation_transformation(&kfm);
matrix_set(H, 0, 0, fix16_one); // z = 1*s
matrix_set(H, 0, 1, 0); // + 0*v
matrix_set(H, 0, 2, 0); // + 0*g
/************************************************************************/
/* set process noise */
/************************************************************************/
mf16 *R = kalman_get_observation_process_noise(&kfm);
matrix_set(R, 0, 0, fix16_half); // var(s)
}
/* set process noise */
/************************************************************************/
mf16 *R = kalman_get_observation_process_noise(&kfm);
matrix_set(R, 0, 0, fix16_half); // var(s)
}
// define measurements.
//
// MATLAB source
// -------------
// s = s + v*T + g*0.5*T^2;
// v = v + g*T;
#define MEAS_COUNT (15)
static fix16_t real_distance[MEAS_COUNT] = {
F16(4.905),
F16(19.62),
F16(44.145),
F16(78.48),
F16(122.63),
F16(176.58),
F16(240.35),
F16(313.92),
F16(397.31),
F16(490.5),
F16(593.51),
F16(706.32),
F16(828.94),
F16(961.38),
F16(1103.6) };
int main(void)
{
/* initialize the core clock and the systick timer */
InitClock();
InitSysTick();
/* initialize the RGB led */
LED_Init();
/* Initialize UART0 */
InitUart0();
/* double rainbow all across the sky */
DoubleFlash();
/* initialize the I2C bus */
I2C_Init();
#if DATA_FUSE_MODE
/* signaling for fusion */
FusionSignal_Init();
#endif // DATA_FUSE_MODE
/* initialize UART fifos */
RingBuffer_Init(&uartInputFifo, &uartInputData, UART_RX_BUFFER_SIZE);
RingBuffer_Init(&uartOutputFifo, &uartOutputData, UART_TX_BUFFER_SIZE);
/* initialize UART0 interrupts */
Uart0_InitializeIrq(&uartInputFifo, &uartOutputFifo);
Uart0_EnableReceiveIrq();
/* initialize I2C arbiter */
InitI2CArbiter();
/* initialize the IMUs */
InitHMC5883L();
InitMPU6050();
// InitMPU6050();
#if ENABLE_MMA8451Q
InitMMA8451Q();
#endif
/* Wait for the config messages to get flushed */
//TrafficLight();
DoubleFlash();
RingBuffer_BlockWhileNotEmpty(&uartOutputFifo);
#if ENABLE_MMA8451Q
/* initialize the MMA8451Q data structure for accelerometer data fetching */
mma8451q_acc_t acc;
MMA8451Q_InitializeData(&acc);
#endif
/* initialize the MPU6050 data structure */
mpu6050_sensor_t accgyrotemp, previous_accgyrotemp;
MPU6050_InitializeData(&accgyrotemp);
MPU6050_InitializeData(&previous_accgyrotemp);
/* initialize the HMC5883L data structure */
hmc5883l_data_t compass, previous_compass;
HMC5883L_InitializeData(&compass);
HMC5883L_InitializeData(&previous_compass);
/* initialize HMC5883L reading */
uint32_t lastHMCRead = 0;
const uint32_t readHMCEvery = 1000 / 75; /* at 75Hz, data come every (1000/75Hz) ms. */
/************************************************************************/
/* Fetch scaler values */
/************************************************************************/
#if DATA_FUSE_MODE
const fix16_t mpu6050_accelerometer_scaler = mpu6050_accelerometer_get_scaler();
const fix16_t mpu6050_gyroscope_scaler = mpu6050_gyroscope_get_scaler();
const fix16_t hmc5883l_magnetometer_scaler = hmc5883l_magnetometer_get_scaler();
#endif // DATA_FUSE_MODE
/************************************************************************/
/* Prepare data fusion */
/************************************************************************/
#if DATA_FUSE_MODE
uint32_t last_transmit_time = 0;
uint32_t last_fusion_time = systemTime();
fusion_initialize();
#endif // DATA_FUSE_MODE
/************************************************************************/
/* Main loop */
/************************************************************************/
for(;;)
{
/* helper variables to track data freshness */
uint_fast8_t have_gyro_data = 0;
uint_fast8_t have_acc_data = 0;
uint_fast8_t have_mag_data = 0;
/************************************************************************/
/* Determine if sensor data fetching is required */
/************************************************************************/
/* helper variables for event processing */
int eventsProcessed = 0;
int readMPU, readHMC;
#if ENABLE_MMA8451Q
int readMMA;
#endif
/* atomic detection of fresh data */
__disable_irq();
#if ENABLE_MMA8451Q
readMMA = poll_mma8451q;
#endif
readMPU = poll_mpu6050;
poll_mma8451q = 0;
poll_mpu6050 = 0;
__enable_irq();
/* detection of HMC read */
/*
* TODO: read synchronized with MPU
*/
readHMC = 0;
uint32_t time = systemTime();
if ((time - lastHMCRead) >= readHMCEvery)
{
readHMC = 1;
lastHMCRead = time;
}
/************************************************************************/
/* Fetching MPU6050 sensor data if required */
/************************************************************************/
/* read accelerometer/gyro */
if (readMPU)
{
LED_BlueOff();
I2CArbiter_Select(MPU6050_I2CADDR);
MPU6050_ReadData(&accgyrotemp);
/* mark event as detected */
eventsProcessed = 1;
/* check for data freshness */
have_acc_data = (accgyrotemp.accel.x != previous_accgyrotemp.accel.x)
|| (accgyrotemp.accel.y != previous_accgyrotemp.accel.y)
|| (accgyrotemp.accel.z != previous_accgyrotemp.accel.z);
have_gyro_data = (accgyrotemp.gyro.x != previous_accgyrotemp.gyro.x)
|| (accgyrotemp.gyro.y != previous_accgyrotemp.gyro.y)
|| (accgyrotemp.gyro.z != previous_accgyrotemp.gyro.z);
/* loop current data --> previous data */
previous_accgyrotemp = accgyrotemp;
}
/************************************************************************/
/* Fetching HMC5883L sensor data if required */
/************************************************************************/
/* read compass data */
if (readHMC)
{
I2CArbiter_Select(HMC5883L_I2CADDR);
HMC5883L_ReadData(&compass);
/* mark event as detected */
eventsProcessed = 1;
/* check for data freshness */
have_mag_data = (compass.x != previous_compass.x)
|| (compass.y != previous_compass.y)
|| (compass.z != previous_compass.z);
/* loop current data --> previous data */
previous_compass = compass;
}
/************************************************************************/
/* Fetching MMA8451Q sensor data if required */
/************************************************************************/
#if ENABLE_MMA8451Q
/* read accelerometer */
if (readMMA)
{
LED_RedOff();
I2CArbiter_Select(MMA8451Q_I2CADDR);
MMA8451Q_ReadAcceleration14bitNoFifo(&acc);
/* mark event as detected */
eventsProcessed = 1;
}
#endif
/************************************************************************/
/* Raw sensor data output over serial */
/************************************************************************/
#if DATA_FETCH_MODE
/* data availability + sanity check
* This sent me on a long bug hunt: Sometimes the interrupt would be raised
* even if not all data registers were written. This always resulted in a
* z data register not being fully written which, in turn, resulted in
* extremely jumpy measurements.
*/
if (readMPU && accgyrotemp.status != 0)
{
/* write data */
uint8_t type = 0x02;
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)accgyrotemp.data, sizeof(accgyrotemp.data), IO_SendByte);
}
/* data availability + sanity check */
if (readHMC && (compass.status & HMC5883L_SR_RDY_MASK) != 0) /* TODO: check if not in lock state */
{
uint8_t type = 0x03;
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)compass.xyz, sizeof(compass.xyz), IO_SendByte);
}
#if ENABLE_MMA8451Q
/* data availability + sanity check */
if (readMMA && acc.status != 0)
{
uint8_t type = 0x01;
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)acc.xyz, sizeof(acc.xyz), IO_SendByte);
}
#endif
#endif // DATA_FETCH_MODE
/************************************************************************/
/* Sensor data fusion */
/************************************************************************/
#if DATA_FUSE_MODE
// if there were sensor data ...
if (eventsProcessed)
{
v3d gyro, acc, mag;
// convert, calibrate and store gyroscope data
if (have_gyro_data)
{
sensor_prepare_mpu6050_gyroscope_data(&gyro, accgyrotemp.gyro.x, accgyrotemp.gyro.y, accgyrotemp.gyro.z, mpu6050_gyroscope_scaler);
fusion_set_gyroscope_v3d(&gyro);
}
// convert, calibrate and store accelerometer data
if (have_acc_data)
{
sensor_prepare_mpu6050_accelerometer_data(&acc, accgyrotemp.accel.x, accgyrotemp.accel.y, accgyrotemp.accel.z, mpu6050_accelerometer_scaler);
fusion_set_accelerometer_v3d(&acc);
}
// convert, calibrate and store magnetometer data
if (have_mag_data)
{
sensor_prepare_hmc5883l_data(&mag, compass.x, compass.y, compass.z, hmc5883l_magnetometer_scaler);
fusion_set_magnetometer_v3d(&mag);
}
// get the time differential
const uint32_t current_time = systemTime();
const fix16_t deltaT_ms = fix16_from_int(current_time - last_fusion_time);
const fix16_t deltaT = fix16_mul(deltaT_ms, F16(0.001));
last_fusion_time = current_time;
FusionSignal_Predict();
// predict the current measurements
fusion_predict(deltaT);
FusionSignal_Update();
// correct the measurements
fusion_update(deltaT);
FusionSignal_Clear();
#if 0
fix16_t yaw, pitch, roll;
fusion_fetch_angles(&roll, &pitch, &yaw);
#if 0
float yawf = fix16_to_float(yaw),
pitchf = fix16_to_float(pitch),
rollf = fix16_to_float(roll);
IO_SendInt16((int16_t)yawf);
IO_SendInt16((int16_t)pitchf);
IO_SendInt16((int16_t)rollf);
IO_SendByteUncommited('\r');
IO_SendByte('\n');
#else
if (current_time - last_transmit_time >= 100)
{
/* write data */
uint8_t type = 42;
fix16_t buffer[3] = { roll, pitch, yaw };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
last_transmit_time = current_time;
}
#endif
#else
if (current_time - last_transmit_time >= 100)
{
/* write data */
switch (output_mode)
{
case RPY:
{
fix16_t roll, pitch, yaw;
fusion_fetch_angles(&roll, &pitch, &yaw);
/* write data */
uint8_t type = 42;
fix16_t buffer[3] = { roll, pitch, yaw };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
case QUATERNION:
{
qf16 orientation;
fusion_fetch_quaternion(&orientation);
uint8_t type = 43;
fix16_t buffer[4] = { orientation.a, orientation.b, orientation.c, orientation.d };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
case QUATERNION_RPY:
{
fix16_t roll, pitch, yaw;
fusion_fetch_angles(&roll, &pitch, &yaw);
qf16 orientation;
fusion_fetch_quaternion(&orientation);
uint8_t type = 44;
fix16_t buffer[7] = { orientation.a, orientation.b, orientation.c, orientation.d, roll, pitch, yaw };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
case SENSORS_RAW:
{
uint8_t type = 0;
fix16_t buffer[6] = { acc.x, acc.y, acc.z, mag.x, mag.y, mag.z };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
}
last_transmit_time = current_time;
}
#endif
}
#endif // DATA_FUSE_MODE
/************************************************************************/
/* Read user data input */
/************************************************************************/
/* as long as there is data in the buffer */
while(!RingBuffer_Empty(&uartInputFifo))
{
/* light one led */
LED_RedOn();
/* fetch byte */
uint8_t data = IO_ReadByte();
output_mode = (output_mode_t)data;
LED_RedOff();
#if 0
/* echo to output */
IO_SendByte(data);
/* mark event as detected */
eventsProcessed = 1;
#endif
}
/************************************************************************/
/* Save energy if you like to */
/************************************************************************/
/* in case of no events, allow a sleep */
if (!eventsProcessed)
{
/*
* Care must be taken with this instruction here, as it can lead
* to a condition where after being woken up (e.g. by the SysTick)
* and looping through, immediately before entering WFI again
* an interrupt would yield a true condition for the branches below.
* In this case this loop would be blocked until the next IRQ,
* which, in case of a 1ms SysTick timer, could be too late.
*
* To counter this behaviour, SysTick has been speed up by factor
* four (0.25ms).
*/
#if 0
__WFI();
#endif
}
}
return 0;
}
void UI::render() {
GD.RestoreContext();
if (game.isOver()) {
GD.ColorRGB(WHITE);
GD.ColorA(128);
GD.Begin(BITMAPS);
GD.BitmapHandle(YOU_DIED_HANDLE);
GD.Cell(0);
GD.BitmapSize(NEAREST, BORDER, BORDER, SCREEN_WIDTH, SCREEN_HEIGHT);
GD.cmd_loadidentity();
GD.cmd_scale(F16(8), F16(8));
GD.cmd_setmatrix();
GD.Vertex2f(0, 0);
} else {
GD.ColorA(0x88);
GD.ColorRGB(GREY);
GD.Begin(EDGE_STRIP_B);
GD.Vertex2ii(0, SCREEN_HEIGHT - 30);
GD.Vertex2ii(56, SCREEN_HEIGHT - 30);
GD.Vertex2ii(76, SCREEN_HEIGHT - 10);
GD.Vertex2ii(SCREEN_WIDTH / 2 - 52, SCREEN_HEIGHT - 10);
GD.Vertex2ii(SCREEN_WIDTH / 2 - 32, SCREEN_HEIGHT - 30);
GD.Vertex2ii(SCREEN_WIDTH / 2 + 32, SCREEN_HEIGHT - 30);
GD.Vertex2ii(SCREEN_WIDTH / 2 + 52, SCREEN_HEIGHT - 10);
GD.Vertex2ii(SCREEN_WIDTH - 76, SCREEN_HEIGHT - 10);
GD.Vertex2ii(SCREEN_WIDTH - 56, SCREEN_HEIGHT - 30);
GD.Vertex2ii(SCREEN_WIDTH - 0, SCREEN_HEIGHT - 30);
GD.Begin(EDGE_STRIP_A);
GD.Vertex2ii(0, 20);
GD.Vertex2ii(80, 20);
GD.Vertex2ii(100, 0);
GD.Vertex2ii(SCREEN_WIDTH - 100, 0);
GD.Vertex2ii(SCREEN_WIDTH - 72, 28);
GD.Vertex2ii(SCREEN_WIDTH, 28);
GD.ColorA(0xFF);
GD.LineWidth(3 * 16);
// prints gameinfo in upper left corner
GD.ColorRGB(WHITE);
GD.cmd_text(4, 4, 16, OPT_SIGNED, "SPACE GAME");
// prints the score in the upper right corner.
GD.cmd_text(SCREEN_WIDTH - 4, 4, 16, OPT_RIGHTX, "SCORE");
GD.cmd_number(SCREEN_WIDTH - 4, 4 + 12, 16, OPT_RIGHTX + 8, game.score);
// prints the health level in the lower left corner
GD.Begin(LINES);
GD.ColorRGB(DARK_GREY);
GD.Vertex2f(16 * 4, 16 * (SCREEN_HEIGHT - 4));
GD.Vertex2f(16 * (SCREEN_WIDTH / 2 - 16), 16 * (SCREEN_HEIGHT - 4));
GD.ColorRGB(COLOR_HEALTH);
GD.Vertex2f(16 * 4, 16 * (SCREEN_HEIGHT - 4));
GD.Vertex2f(
((16 * (SCREEN_WIDTH / 2 - 16)) / MAX_PLAYER_HEALTH)
* player.health, 16 * (SCREEN_HEIGHT - 4));
GD.cmd_text(4, SCREEN_HEIGHT - 26, 16, OPT_SIGNED, "Health");
GD.cmd_number(4, SCREEN_HEIGHT - 16, 16, OPT_SIGNED + 4, player.health);
if (player.health < 100) {
GD.cmd_text(SCREEN_WIDTH / 2 - 16, 100, 16, OPT_RIGHTX,
"LOW HEALTH!");
}
// prints energy level in the lower right corner
GD.Begin(LINES);
GD.ColorRGB(DARK_GREY);
GD.Vertex2f(16 * (SCREEN_WIDTH - 4), 16 * (SCREEN_HEIGHT - 4));
GD.Vertex2f(16 * (SCREEN_WIDTH / 2 + 16), 16 * (SCREEN_HEIGHT - 4));
GD.ColorRGB(COLOR_ENERGY);
GD.Vertex2f(16 * (SCREEN_WIDTH - 4), 16 * (SCREEN_HEIGHT - 4));
GD.Vertex2f(
(16 * SCREEN_WIDTH)
- (16 * (SCREEN_WIDTH / 2 - 16) / MAX_PLAYER_ENERGY)
* player.energy, 16 * (SCREEN_HEIGHT - 4));
GD.cmd_text( SCREEN_WIDTH - 4, SCREEN_HEIGHT - 26, 16, OPT_RIGHTX,
"Energy");
GD.cmd_number(SCREEN_WIDTH - 4, SCREEN_HEIGHT - 16, 16, OPT_RIGHTX + 4,
player.energy);
if (player.energy < 100) {
GD.cmd_text(SCREEN_WIDTH / 2 + 16, 100, 16, OPT_SIGNED,
"LOW ENERGY!");
}
// prints the height in the lower middle.
GD.cmd_text(SCREEN_WIDTH / 2 - 4, SCREEN_HEIGHT - 26, 16, OPT_CENTERX,
"Height");
// GD.cmd_number(SCREEN_WIDTH/2-4, SCREEN_HEIGHT - 16, 16, OPT_CENTERX + 4, player.height);
GD.cmd_number(SCREEN_WIDTH / 2 - 4, SCREEN_HEIGHT - 16, 16,
OPT_CENTERX + 4, player.height);
}
#if DEBUG
GD.RestoreContext();
GD.ColorRGB(WHITE);
GD.ColorA(255);
GD.cmd_text(4, 16, 16, OPT_SIGNED, "X:");
GD.cmd_text(4, 28, 16, OPT_SIGNED, "Y:");
GD.cmd_number(20, 4 + 12, 16, OPT_SIGNED, player.getPosition().x);
GD.cmd_number(20, 4 + 24, 16, OPT_SIGNED, player.getPosition().y);
//prints FPS
GD.cmd_text(4, 50, 16, OPT_SIGNED, "FPS:");
GD.cmd_number(36, 50, 16, OPT_SIGNED, 1 / timer.getDeltaTime());
//prints speed
GD.cmd_text(4, 70, 16, OPT_SIGNED, "UPS:"); // units per sec
GD.cmd_number(36, 70, 16, OPT_SIGNED, player.getVelocity().length());
GD.cmd_text(4, 150, 16, OPT_SIGNED, "B1: ");
if (input.getButton1()) {
GD.cmd_text(30, 150, 16, OPT_SIGNED, "TRUE");
} else {
GD.cmd_text(30, 150, 16, OPT_SIGNED, "FALSE");
}
GD.cmd_text(4, 170, 16, OPT_SIGNED, "B2: ");
if (input.getButton2()) {
GD.cmd_text(30, 170, 16, OPT_SIGNED, "TRUE");
} else {
GD.cmd_text(30, 170, 16, OPT_SIGNED, "FALSE");
}
GD.cmd_text(4, 190, 16, OPT_SIGNED, "B3: ");
if (input.getButton3()) {
GD.cmd_text(30, 190, 16, OPT_SIGNED, "TRUE");
} else {
GD.cmd_text(30, 190, 16, OPT_SIGNED, "FALSE");
}
GD.cmd_text(4, 210, 16, OPT_SIGNED, "B4: ");
if (input.getButton4()) {
GD.cmd_text(30, 210, 16, OPT_SIGNED, "TRUE");
} else {
GD.cmd_text(30, 210, 16, OPT_SIGNED, "FALSE");
}
#endif
GD.RestoreContext();
}
int main () {
//===============================================================//
//========= PROBLEM SETUP =======================//
//===============================================================//
int d = 4;
int j0 = 2;
cout << "Wavelets: d = " << d << ", j0 = " << j0 << endl << endl;
/// Basis initialization
_Basis basis(d, j0);
Basis2D basis2d(basis, basis);
/*
for (int i = 1; i<= 6; ++i) {
DataType u;
std::string snapshot;
std::string plot;
snapshot = "training_data_toy/snap/snap_";
plot = "plot_";
snapshot += std::to_string(i);
plot += std::to_string(i);
snapshot += ".txt";
std::cout << "Reading snapshot from file " << snapshot << std::endl;
std::cout << "Printing plot to file " << plot << std::endl;
readCoeffVector2D(u, snapshot.c_str());
precon _p;
plot2D(basis2d, u, _p,
zero,
0., 1.,
0., 1.,
1e-02,
plot.c_str());
}
exit(0);
*/
/// Initialization of operators
DenseVectorT no_singPts;
Function<T> zero_Fct(zero_fct,no_singPts);
// Bilinear Forms
Identity1D IdentityBil(basis);
Laplace1D LaplaceBil(basis);
RefIdentity1D RefIdentityBil(basis.refinementbasis);
RefLaplace1D RefLaplaceBil(basis.refinementbasis);
/// Initialization of local operator
LOp_Id1D lOp_Id1D (basis, basis, RefIdentityBil, IdentityBil);
LOp_Lapl1D lOp_Lapl1D (basis, basis, RefLaplaceBil, LaplaceBil);
LOp_Id_Id_2D localIdentityIdentityOp2D (lOp_Id1D, lOp_Id1D);
LOp_Id_Lapl_2D localIdentityLaplaceOp2D (lOp_Id1D, lOp_Lapl1D);
LOp_Lapl_Id_2D localLaplaceIdentityOp2D (lOp_Lapl1D, lOp_Id1D);
localIdentityIdentityOp2D.setJ(9);
localIdentityLaplaceOp2D.setJ(9);
localLaplaceIdentityOp2D.setJ(9);
/// Initialization of preconditioner
Prec2D prec(basis2d);
NoPrec2D noPrec;
/// Initialization of rhs
/// Right Hand Side:
/// No Singular Supports in both dimensions
DenseVectorT sing_support;
/// Forcing Functions
Function2D<T> F1_Fct(f1, sing_support, sing_support);
Function2D<T> F2_Fct(f2, sing_support, sing_support);
Function2D<T> F3_Fct(f3, sing_support, sing_support);
Function2D<T> F4_Fct(f4, sing_support, sing_support);
Function2D<T> F5_Fct(f5, sing_support, sing_support);
Function2D<T> F6_Fct(f6, sing_support, sing_support);
Function2D<T> F7_Fct(f7, sing_support, sing_support);
Function2D<T> F8_Fct(f8, sing_support, sing_support);
Function2D<T> F9_Fct(f9, sing_support, sing_support);
Function2D<T> F10_Fct(f10, sing_support, sing_support);
Function2D<T> F11_Fct(f11, sing_support, sing_support);
Function2D<T> F12_Fct(f12, sing_support, sing_support);
Function2D<T> F13_Fct(f13, sing_support, sing_support);
Function2D<T> F14_Fct(f14, sing_support, sing_support);
Function2D<T> F15_Fct(f15, sing_support, sing_support);
Function2D<T> F16_Fct(f16, sing_support, sing_support);
Function2D<T> F17_Fct(f17, sing_support, sing_support);
Function2D<T> F18_Fct(f18, sing_support, sing_support);
Function2D<T> F19_Fct(f19, sing_support, sing_support);
Function2D<T> F20_Fct(f20, sing_support, sing_support);
Function2D<T> F21_Fct(f21, sing_support, sing_support);
Function2D<T> F22_Fct(f22, sing_support, sing_support);
Function2D<T> F23_Fct(f23, sing_support, sing_support);
Function2D<T> F24_Fct(f24, sing_support, sing_support);
Function2D<T> F25_Fct(f25, sing_support, sing_support);
Function2D<T> F26_Fct(f26, sing_support, sing_support);
Function2D<T> F27_Fct(f27, sing_support, sing_support);
Function2D<T> F28_Fct(f28, sing_support, sing_support);
Function2D<T> F29_Fct(f29, sing_support, sing_support);
Function2D<T> F30_Fct(f30, sing_support, sing_support);
Function2D<T> F31_Fct(f31, sing_support, sing_support);
Function2D<T> F32_Fct(f32, sing_support, sing_support);
Function2D<T> F33_Fct(f33, sing_support, sing_support);
Function2D<T> F34_Fct(f34, sing_support, sing_support);
Function2D<T> F35_Fct(f35, sing_support, sing_support);
Function2D<T> F36_Fct(f36, sing_support, sing_support);
Function2D<T> F37_Fct(f37, sing_support, sing_support);
Function2D<T> F38_Fct(f38, sing_support, sing_support);
Function2D<T> F39_Fct(f39, sing_support, sing_support);
Function2D<T> F40_Fct(f40, sing_support, sing_support);
Function2D<T> F41_Fct(f41, sing_support, sing_support);
Function2D<T> F42_Fct(f42, sing_support, sing_support);
Function2D<T> F43_Fct(f43, sing_support, sing_support);
Function2D<T> F44_Fct(f44, sing_support, sing_support);
Function2D<T> F45_Fct(f45, sing_support, sing_support);
Function2D<T> F46_Fct(f46, sing_support, sing_support);
Function2D<T> F47_Fct(f47, sing_support, sing_support);
Function2D<T> F48_Fct(f48, sing_support, sing_support);
Function2D<T> F49_Fct(f49, sing_support, sing_support);
RhsIntegral2D rhs_1(basis2d, F1_Fct, 100);
RhsIntegral2D rhs_2(basis2d, F2_Fct, 100);
RhsIntegral2D rhs_3(basis2d, F3_Fct, 100);
RhsIntegral2D rhs_4(basis2d, F4_Fct, 100);
RhsIntegral2D rhs_5(basis2d, F5_Fct, 100);
RhsIntegral2D rhs_6(basis2d, F6_Fct, 100);
RhsIntegral2D rhs_7(basis2d, F7_Fct, 100);
RhsIntegral2D rhs_8(basis2d, F8_Fct, 100);
RhsIntegral2D rhs_9(basis2d, F9_Fct, 100);
RhsIntegral2D rhs_10(basis2d, F10_Fct, 100);
RhsIntegral2D rhs_11(basis2d, F11_Fct, 100);
RhsIntegral2D rhs_12(basis2d, F12_Fct, 100);
RhsIntegral2D rhs_13(basis2d, F13_Fct, 100);
RhsIntegral2D rhs_14(basis2d, F14_Fct, 100);
RhsIntegral2D rhs_15(basis2d, F15_Fct, 100);
RhsIntegral2D rhs_16(basis2d, F16_Fct, 100);
RhsIntegral2D rhs_17(basis2d, F17_Fct, 100);
RhsIntegral2D rhs_18(basis2d, F18_Fct, 100);
RhsIntegral2D rhs_19(basis2d, F19_Fct, 100);
RhsIntegral2D rhs_20(basis2d, F20_Fct, 100);
RhsIntegral2D rhs_21(basis2d, F21_Fct, 100);
RhsIntegral2D rhs_22(basis2d, F22_Fct, 100);
RhsIntegral2D rhs_23(basis2d, F23_Fct, 100);
RhsIntegral2D rhs_24(basis2d, F24_Fct, 100);
RhsIntegral2D rhs_25(basis2d, F25_Fct, 100);
RhsIntegral2D rhs_26(basis2d, F26_Fct, 100);
RhsIntegral2D rhs_27(basis2d, F27_Fct, 100);
RhsIntegral2D rhs_28(basis2d, F28_Fct, 100);
RhsIntegral2D rhs_29(basis2d, F29_Fct, 100);
RhsIntegral2D rhs_30(basis2d, F30_Fct, 100);
RhsIntegral2D rhs_31(basis2d, F31_Fct, 100);
RhsIntegral2D rhs_32(basis2d, F32_Fct, 100);
RhsIntegral2D rhs_33(basis2d, F33_Fct, 100);
RhsIntegral2D rhs_34(basis2d, F34_Fct, 100);
RhsIntegral2D rhs_35(basis2d, F35_Fct, 100);
RhsIntegral2D rhs_36(basis2d, F36_Fct, 100);
RhsIntegral2D rhs_37(basis2d, F37_Fct, 100);
RhsIntegral2D rhs_38(basis2d, F38_Fct, 100);
RhsIntegral2D rhs_39(basis2d, F39_Fct, 100);
RhsIntegral2D rhs_40(basis2d, F40_Fct, 100);
RhsIntegral2D rhs_41(basis2d, F41_Fct, 100);
RhsIntegral2D rhs_42(basis2d, F42_Fct, 100);
RhsIntegral2D rhs_43(basis2d, F43_Fct, 100);
RhsIntegral2D rhs_44(basis2d, F44_Fct, 100);
RhsIntegral2D rhs_45(basis2d, F45_Fct, 100);
RhsIntegral2D rhs_46(basis2d, F46_Fct, 100);
RhsIntegral2D rhs_47(basis2d, F47_Fct, 100);
RhsIntegral2D rhs_48(basis2d, F48_Fct, 100);
RhsIntegral2D rhs_49(basis2d, F49_Fct, 100);
Rhs F1(rhs_1,noPrec);
Rhs F2(rhs_2,noPrec);
Rhs F3(rhs_3,noPrec);
Rhs F4(rhs_4,noPrec);
Rhs F5(rhs_5,noPrec);
Rhs F6(rhs_6,noPrec);
Rhs F7(rhs_7,noPrec);
Rhs F8(rhs_8,noPrec);
Rhs F9(rhs_9,noPrec);
Rhs F10(rhs_10,noPrec);
Rhs F11(rhs_11,noPrec);
Rhs F12(rhs_12,noPrec);
Rhs F13(rhs_13,noPrec);
Rhs F14(rhs_14,noPrec);
Rhs F15(rhs_15,noPrec);
Rhs F16(rhs_16,noPrec);
Rhs F17(rhs_17,noPrec);
Rhs F18(rhs_18,noPrec);
Rhs F19(rhs_19,noPrec);
Rhs F20(rhs_20,noPrec);
Rhs F21(rhs_21,noPrec);
Rhs F22(rhs_22,noPrec);
Rhs F23(rhs_23,noPrec);
Rhs F24(rhs_24,noPrec);
Rhs F25(rhs_25,noPrec);
Rhs F26(rhs_26,noPrec);
Rhs F27(rhs_27,noPrec);
Rhs F28(rhs_28,noPrec);
Rhs F29(rhs_29,noPrec);
Rhs F30(rhs_30,noPrec);
Rhs F31(rhs_31,noPrec);
Rhs F32(rhs_32,noPrec);
Rhs F33(rhs_33,noPrec);
Rhs F34(rhs_34,noPrec);
Rhs F35(rhs_35,noPrec);
Rhs F36(rhs_36,noPrec);
Rhs F37(rhs_37,noPrec);
Rhs F38(rhs_38,noPrec);
Rhs F39(rhs_39,noPrec);
Rhs F40(rhs_40,noPrec);
Rhs F41(rhs_41,noPrec);
Rhs F42(rhs_42,noPrec);
Rhs F43(rhs_43,noPrec);
Rhs F44(rhs_44,noPrec);
Rhs F45(rhs_45,noPrec);
Rhs F46(rhs_46,noPrec);
Rhs F47(rhs_47,noPrec);
Rhs F48(rhs_48,noPrec);
Rhs F49(rhs_49,noPrec);
//===============================================================//
//=============== RB SETUP =====================================//
//===============================================================//
// Affine Decompositions:
// Left Hand Side
vector<ThetaStructure<ParamType>::ThetaFct> lhs_theta_fcts;
lhs_theta_fcts.push_back(no_theta);
ThetaStructure<ParamType> lhs_theta(lhs_theta_fcts);
vector<AbstractLocalOperator2D<T>* > lhs_ops;
lhs_ops.push_back(&localLaplaceIdentityOp2D);
lhs_ops.push_back(&localIdentityLaplaceOp2D);
Flex_COp_2D A(lhs_ops);
vector<Flex_COp_2D*> ops_vec;
ops_vec.push_back(&A);
Affine_Op_2D affine_lhs(lhs_theta, ops_vec);
H1_InnProd_2D innprod(localIdentityIdentityOp2D, localLaplaceIdentityOp2D, localIdentityLaplaceOp2D);
// Right Hand Side
vector<ThetaStructure<ParamType>::ThetaFct> rhs_theta_fcts;
rhs_theta_fcts.push_back(theta_chi_1);
rhs_theta_fcts.push_back(theta_chi_2);
rhs_theta_fcts.push_back(theta_chi_3);
rhs_theta_fcts.push_back(theta_chi_4);
rhs_theta_fcts.push_back(theta_chi_5);
rhs_theta_fcts.push_back(theta_chi_6);
rhs_theta_fcts.push_back(theta_chi_7);
rhs_theta_fcts.push_back(theta_chi_8);
rhs_theta_fcts.push_back(theta_chi_9);
rhs_theta_fcts.push_back(theta_chi_10);
rhs_theta_fcts.push_back(theta_chi_11);
rhs_theta_fcts.push_back(theta_chi_12);
rhs_theta_fcts.push_back(theta_chi_13);
rhs_theta_fcts.push_back(theta_chi_14);
rhs_theta_fcts.push_back(theta_chi_15);
rhs_theta_fcts.push_back(theta_chi_16);
rhs_theta_fcts.push_back(theta_chi_17);
rhs_theta_fcts.push_back(theta_chi_18);
rhs_theta_fcts.push_back(theta_chi_19);
rhs_theta_fcts.push_back(theta_chi_20);
rhs_theta_fcts.push_back(theta_chi_21);
rhs_theta_fcts.push_back(theta_chi_22);
rhs_theta_fcts.push_back(theta_chi_23);
rhs_theta_fcts.push_back(theta_chi_24);
rhs_theta_fcts.push_back(theta_chi_25);
rhs_theta_fcts.push_back(theta_chi_26);
rhs_theta_fcts.push_back(theta_chi_27);
rhs_theta_fcts.push_back(theta_chi_28);
rhs_theta_fcts.push_back(theta_chi_29);
rhs_theta_fcts.push_back(theta_chi_30);
rhs_theta_fcts.push_back(theta_chi_31);
rhs_theta_fcts.push_back(theta_chi_32);
rhs_theta_fcts.push_back(theta_chi_33);
rhs_theta_fcts.push_back(theta_chi_34);
rhs_theta_fcts.push_back(theta_chi_35);
rhs_theta_fcts.push_back(theta_chi_36);
rhs_theta_fcts.push_back(theta_chi_37);
rhs_theta_fcts.push_back(theta_chi_38);
rhs_theta_fcts.push_back(theta_chi_39);
rhs_theta_fcts.push_back(theta_chi_40);
rhs_theta_fcts.push_back(theta_chi_41);
rhs_theta_fcts.push_back(theta_chi_42);
rhs_theta_fcts.push_back(theta_chi_43);
rhs_theta_fcts.push_back(theta_chi_44);
rhs_theta_fcts.push_back(theta_chi_45);
rhs_theta_fcts.push_back(theta_chi_46);
rhs_theta_fcts.push_back(theta_chi_47);
rhs_theta_fcts.push_back(theta_chi_48);
rhs_theta_fcts.push_back(theta_chi_49);
ThetaStructure<ParamType> rhs_theta(rhs_theta_fcts);
vector<Rhs*> rhs_fcts;
rhs_fcts.push_back(&F1);
rhs_fcts.push_back(&F2);
rhs_fcts.push_back(&F3);
rhs_fcts.push_back(&F4);
rhs_fcts.push_back(&F5);
rhs_fcts.push_back(&F6);
rhs_fcts.push_back(&F7);
rhs_fcts.push_back(&F8);
rhs_fcts.push_back(&F9);
rhs_fcts.push_back(&F10);
rhs_fcts.push_back(&F11);
rhs_fcts.push_back(&F12);
rhs_fcts.push_back(&F13);
rhs_fcts.push_back(&F14);
rhs_fcts.push_back(&F15);
rhs_fcts.push_back(&F16);
rhs_fcts.push_back(&F17);
rhs_fcts.push_back(&F18);
rhs_fcts.push_back(&F19);
rhs_fcts.push_back(&F20);
rhs_fcts.push_back(&F21);
rhs_fcts.push_back(&F22);
rhs_fcts.push_back(&F23);
rhs_fcts.push_back(&F24);
rhs_fcts.push_back(&F25);
rhs_fcts.push_back(&F26);
rhs_fcts.push_back(&F27);
rhs_fcts.push_back(&F28);
rhs_fcts.push_back(&F29);
rhs_fcts.push_back(&F30);
rhs_fcts.push_back(&F31);
rhs_fcts.push_back(&F32);
rhs_fcts.push_back(&F33);
rhs_fcts.push_back(&F34);
rhs_fcts.push_back(&F35);
rhs_fcts.push_back(&F36);
rhs_fcts.push_back(&F37);
rhs_fcts.push_back(&F38);
rhs_fcts.push_back(&F39);
rhs_fcts.push_back(&F40);
rhs_fcts.push_back(&F41);
rhs_fcts.push_back(&F42);
rhs_fcts.push_back(&F43);
rhs_fcts.push_back(&F44);
rhs_fcts.push_back(&F45);
rhs_fcts.push_back(&F46);
rhs_fcts.push_back(&F47);
rhs_fcts.push_back(&F48);
rhs_fcts.push_back(&F49);
Affine_Rhs_2D affine_rhs(rhs_theta, rhs_fcts);
RieszF_Rhs_2D rieszF_rhs(rhs_fcts);
RieszA_Rhs_2D rieszA_rhs(ops_vec);
// Right Hand Sides for direct Riesz Representor
AffineA_Rhs_2D affineA_rhs(lhs_theta, lhs_ops);
RieszRes_Rhs_2D rieszRes_rhs(affineA_rhs, affine_rhs);
//===============================================================//
//=============== AWGM =========================================//
//===============================================================//
IS_Parameters is_parameters;
AWGM_Parameters awgm_truth_parameters, awgm_riesz_f_parameters, awgm_riesz_a_parameters, awgm_riesz_res_parameters;
awgm_truth_parameters.max_its = 1e+03;
is_parameters.adaptive_tol = true;
is_parameters.init_tol = 0.0001;
is_parameters.res_reduction = 0.01;
//----------- Solver ---------------- //
T gamma = 0.2;
IndexSet<Index2D> Lambda;
getSparseGridIndexSet(basis2d, Lambda, 1, 0, gamma);
MT_AWGM_Truth awgm_u(basis2d, affine_lhs, affine_rhs, prec, awgm_truth_parameters, is_parameters);
awgm_u.set_sol(dummy);
awgm_u.awgm_params.tol = 1e-04;
awgm_u.awgm_params.max_basissize = 1e+06;
awgm_u.set_initial_indexset(Lambda);
MT_AWGM_Riesz_F awgm_rieszF(basis2d, innprod, rieszF_rhs, prec, awgm_riesz_f_parameters, is_parameters);
awgm_rieszF.set_sol(dummy);
awgm_rieszF.set_initial_indexset(Lambda);
awgm_rieszF.awgm_params.tol = 5e-04;
awgm_rieszF.awgm_params.info_filename = "stempel_rieszF_conv_info.txt";
MT_AWGM_Riesz_A awgm_rieszA(basis2d, innprod, rieszA_rhs, prec, awgm_riesz_a_parameters, is_parameters);
awgm_rieszA.set_sol(dummy);
awgm_rieszA.set_initial_indexset(Lambda);
awgm_rieszA.awgm_params.tol = 5e-04;
awgm_rieszA.awgm_params.info_filename = "stempel_rieszA_conv_info.txt";
MT_AWGM_Riesz_Res awgm_rieszRes(basis2d, innprod, rieszRes_rhs, prec, awgm_riesz_res_parameters, is_parameters);
awgm_rieszRes.set_sol(dummy);
awgm_rieszRes.set_initial_indexset(Lambda);
awgm_rieszRes.awgm_params.tol = 5e-04;
awgm_rieszRes.awgm_params.print_info = false;
MTTruthSolver rb_truth(awgm_u, awgm_rieszF, awgm_rieszA, &awgm_rieszRes, innprod, affine_lhs, rieszF_rhs);
//----------- RB System ---------------- //
RB_Model rb_system(lhs_theta, rhs_theta);
rb_system.rb_params.ref_param = {{1.}};
rb_system.rb_params.call = call_cg;
//----------- RB Base ---------------- //
RB_BaseModel rb_base(rb_system, rb_truth, Lambda);
ParamType mu_min = {{0}};
ParamType mu_max = {{1}};
rb_base.greedy_params.training_type = weak;
rb_base.greedy_params.tol = 1e-05;
rb_base.greedy_params.min_param = mu_min;
rb_base.greedy_params.max_param = mu_max;
rb_base.greedy_params.Nmax = 6;
rb_base.greedy_params.nb_training_params = {{nb_stempel}};
rb_base.greedy_params.log_scaling = {{0}};
rb_base.greedy_params.print_file = "toy_greedy_info.txt";
rb_base.greedy_params.trainingdata_folder = "training_data_toy";
rb_base.greedy_params.print_paramset = true;
rb_base.greedy_params.erase_snapshot_params = false;
rb_base.greedy_params.orthonormalize_bfs = true;
rb_base.greedy_params.tighten_tol = true;
rb_base.greedy_params.snapshot_tol_red_crit = repeated_param;
rb_base.greedy_params.tighten_tol_rieszA = false;
rb_base.greedy_params.tighten_tol_rieszF = false;
rb_base.greedy_params.tighten_tol_reduction = 0.1,
rb_base.greedy_params.update_snapshot = true;
rb_base.greedy_params.update_rieszF = false;
rb_base.greedy_params.update_rieszA = false;
rb_base.greedy_params.test_estimator_equivalence = false;
rb_base.greedy_params.write_direct_representors = false;
rb_base.greedy_params.riesz_constant_X = 5.6;
rb_base.greedy_params.riesz_constant_Y = 2;
cout << "Parameters Training: " << std::endl << std::endl;
rb_base.greedy_params.print();
rb_system.rb_params.print();
cout << "Parameters Truth Solver: " << std::endl << std::endl;
awgm_u.awgm_params.print();
awgm_u.is_params.print();
cout << "Parameters Riesz Solver F : " << std::endl << std::endl;
awgm_rieszF.awgm_params.print();
cout << "Parameters Riesz Solver A : " << std::endl << std::endl;
awgm_rieszA.awgm_params.print();
cout << "Parameters Riesz Solver Res : " << std::endl << std::endl;
awgm_rieszRes.awgm_params.print();
//rb_system.read_rb_data("training_data_toy");
//rb_base.read_basisfunctions("training_data_toy/bf");
/*for (int i=0; i<rb_base.n_bf(); ++i) {
std::cout << " i = " << i+1 << std::endl;
std::cout << rb_base.rb_basisfunctions[i].size() << std::endl;
}*/
//rb_base.read_rieszrepresentors("training_data_toy/representors");
//rb_base.calculate_Riesz_RHS_information(true);
//rb_base.calc_rb_data();
//rb_system.write_rb_data("training_data_toy");
rb_base.train_Greedy();
std::cout << "toy_offline exited normally\n";
return 0;
}
void
main(void)
{
hardware_init();
uint32_t color_idx;
for (color_idx = 0; color_idx < TEAPOT_POLYGON_CNT; color_idx++) {
uint32_t idx;
idx = color_idx;
uint32_t polygon_idx;
fix16_vector4_t *polygon[4];
for (polygon_idx = 0; polygon_idx < 4; polygon_idx++) {
uint32_t vertex_idx;
vertex_idx = teapot_indices[(4 * idx) + polygon_idx];
fix16_vector4_t *vertex;
vertex = &teapot_vertices[vertex_idx];
polygon[polygon_idx] = vertex;
}
fix16_t avg;
avg = fix16_add(fix16_mul(fix16_add(
fix16_add(polygon[0]->z, polygon[1]->z),
fix16_add(polygon[2]->z, polygon[3]->z)),
F16(1.0f / 4.0f)), F16(0.1f));
uint16_t color;
color = fix16_to_int(fix16_add(
fix16_mul(fix16_abs(avg), F16(255.0f)),
F16(16.0f)));
colors[color_idx] = RGB888_TO_RGB555(color, color, color);
}
ot_init();
matrix_stack_init();
matrix_stack_mode(MATRIX_STACK_MODE_PROJECTION);
fix16_t ratio;
ratio = F16((float)SCREEN_WIDTH / (float)SCREEN_HEIGHT);
matrix_stack_orthographic_project(-fix16_mul(F16(1.0f), ratio),
fix16_mul(F16(1.0f), ratio),
F16(1.0f), F16(-1.0f),
F16(1.0f), F16(1.0f));
matrix_stack_mode(MATRIX_STACK_MODE_MODEL_VIEW);
matrix_stack_translate(F16(0.0f), F16(0.0f), F16(-10.0f));
vdp1_cmdt_list_begin(0); {
struct vdp1_cmdt_local_coord local_coord;
local_coord.lc_coord.x = SCREEN_WIDTH / 2;
local_coord.lc_coord.y = SCREEN_HEIGHT / 2;
vdp1_cmdt_local_coord_set(&local_coord);
vdp1_cmdt_end();
} vdp1_cmdt_list_end(0);
struct vdp1_cmdt_polygon polygon;
memset(&polygon, 0x00, sizeof(polygon));
vdp2_tvmd_vblank_out_wait();
vdp2_tvmd_vblank_in_wait();
fix16_t angle = F16(0.0f);
while (true) {
vdp2_tvmd_vblank_out_wait();
// Update
matrix_stack_mode(MATRIX_STACK_MODE_MODEL_VIEW);
matrix_stack_push(); {
matrix_stack_rotate(angle, 0);
matrix_stack_rotate(angle, 1);
matrix_stack_rotate(angle, 2);
angle = fix16_add(angle, F16(-1.0f));
model_polygon_project(teapot_vertices, teapot_indices,
teapot_normals, TEAPOT_POLYGON_CNT);
} matrix_stack_pop();
vdp1_cmdt_list_begin(1); {
int32_t idx;
for (idx = OT_PRIMITIVE_BUCKETS - 1; idx >= 0; idx--) {
/* Skip empty buckets */
if (ot_bucket_empty(idx)) {
continue;
}
#if POLYGON_SORT == 1
ot_bucket_primitive_sort(idx & (OT_PRIMITIVE_BUCKETS - 1),
OT_PRIMITIVE_BUCKET_SORT_INSERTION);
#endif
#if RENDER == 1
struct ot_primitive *otp;
TAILQ_FOREACH (otp, ot_bucket(idx), otp_entries) {
polygon.cp_color = otp->otp_color;
polygon.cp_mode.transparent_pixel = true;
polygon.cp_mode.end_code = true;
polygon.cp_vertex.a.x = otp->otp_coords[0].x;
polygon.cp_vertex.a.y = otp->otp_coords[0].y;
polygon.cp_vertex.b.x = otp->otp_coords[1].x;
polygon.cp_vertex.b.y = otp->otp_coords[1].y;
polygon.cp_vertex.c.x = otp->otp_coords[2].x;
polygon.cp_vertex.c.y = otp->otp_coords[2].y;
polygon.cp_vertex.d.x = otp->otp_coords[3].x;
polygon.cp_vertex.d.y = otp->otp_coords[3].y;
vdp1_cmdt_polygon_draw(&polygon);
}
#endif
/* Clear OT bucket */
ot_bucket_init(idx);
}
vdp1_cmdt_end();
} vdp1_cmdt_list_end(1);
vdp2_tvmd_vblank_in_wait();
// Draw
vdp1_cmdt_list_commit();
}
static void selection_changed(GtkWidget* widget, gpointer data) {
GtkTreeIter iter;
void* voidptr;
if (gtk_tree_selection_get_selected(GTK_TREE_SELECTION(widget), NULL, &iter)) {
gtk_tree_model_get(GTK_TREE_MODEL(listStore), &iter, 1, &voidptr, -1);
if (pocketnes_is_romheader(voidptr)) {
pocketnes_romheader* ptr = (pocketnes_romheader*)voidptr;
char buf[256];
sprintf(buf, "Size: %u bytes ", F32(ptr->filesize));
gtk_label_set_text(GTK_LABEL(lblSize), buf);
gtk_label_set_text(GTK_LABEL(lblType), "Type: NES ROM (PocketNES)");
sprintf(buf, "Title: %.32s", ptr->name);
gtk_label_set_text(GTK_LABEL(lblTitle), buf);
gtk_label_set_text(GTK_LABEL(lblDataHash), "");
gtk_widget_hide(dataHashColor);
gtk_widget_set_sensitive(btnExport, true);
gtk_widget_set_sensitive(btnReplace, false);
show_standard_rows();
gtk_label_set_text(GTK_LABEL(lblUncompSize), "");
gtk_label_set_text(GTK_LABEL(lblFramecount), "");
sprintf(buf, "ROM checksum: %08X", (unsigned int)pocketnes_get_checksum(ptr+1));
gtk_label_set_text(GTK_LABEL(lblChecksum), buf);
} else if (gb_is_rom(voidptr)) {
char buf[256];
sprintf(buf, "Size: %u bytes ", gb_rom_size(voidptr));
gtk_label_set_text(GTK_LABEL(lblSize), buf);
gtk_label_set_text(GTK_LABEL(lblType), "Type: Game Boy ROM (Goomba)");
sprintf(buf, "Title: %.16s", gb_get_title(voidptr, NULL));
gtk_label_set_text(GTK_LABEL(lblTitle), buf);
gtk_label_set_text(GTK_LABEL(lblDataHash), "");
gtk_widget_hide(dataHashColor);
gtk_widget_set_sensitive(btnExport, true);
gtk_widget_set_sensitive(btnReplace, false);
show_standard_rows();
gtk_label_set_text(GTK_LABEL(lblUncompSize), "");
gtk_label_set_text(GTK_LABEL(lblFramecount), "");
sprintf(buf, "ROM checksum: %08X", (unsigned int)gb_get_checksum(voidptr));
gtk_label_set_text(GTK_LABEL(lblChecksum), buf);
}
else if (stateheader_plausible(voidptr)) {
stateheader* ptr = (stateheader*)voidptr;
char buf[256];
sprintf(buf, "Size: %u bytes ", F16(ptr->size));
gtk_label_set_text(GTK_LABEL(lblSize), buf);
gtk_label_set_text(GTK_LABEL(lblType),
ptr->type == GOOMBA_STATESAVE ? "Type: Savestate"
: ptr->type == GOOMBA_SRAMSAVE ? "Type: SRAM"
: ptr->type == GOOMBA_CONFIGSAVE ? "Type: Configuration"
: "Type:");
if (ptr->size > sizeof(stateheader)) {
uint64_t ck = goomba_compressed_data_checksum(ptr, 3);
sprintf(buf, "Hash of compressed data: %6X", (unsigned int)ck);
gtk_label_set_text(GTK_LABEL(lblDataHash), buf);
GdkColor color;
color.red = 0x101 * ((ck >> 16) & 0xFF);
color.green = 0x101 * ((ck >> 8) & 0xFF);
color.blue = 0x101 * (ck & 0xFF);
gtk_widget_show(dataHashColor);
gtk_widget_modify_bg(dataHashColor, GTK_STATE_NORMAL, &color);
} else {
