bool RayTracer::refract(SbVec3f *ray_direction, SbVec3f *normal_at_intersection, SbVec3f *T){
float under_sqrt = 0;
float refractive_index = 0;
SbVec3f N, d, V;
N = *normal_at_intersection;
d = *ray_direction;
V = -1 * d;
d.normalize();
N.normalize();
V.normalize();
refractive_index = 1.0 / RI;
if (N.dot(d) >= 0 ){
//std::cout<<"out";
N = -1 * N;
N.normalize();
refractive_index = RI / 1.0 ;
}
under_sqrt = should_transmit(&d, &N, refractive_index);
if(under_sqrt > 0){
float tempTerm;
tempTerm = ((refractive_index * N.dot(V)) - sqrt(under_sqrt));
*T = (tempTerm * N ) - (refractive_index * V);
return true;
}
return false;
}
void
Mavlink::resend_message(mavlink_message_t *msg)
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
pthread_mutex_lock(&_send_mutex);
unsigned buf_free = get_free_tx_buf();
_last_write_try_time = hrt_absolute_time();
unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
/* check if there is space in the buffer, let it overflow else */
if (buf_free < packet_len) {
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
pthread_mutex_unlock(&_send_mutex);
return;
}
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header and payload */
memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len);
/* checksum */
buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8);
#ifndef __PX4_POSIX
/* send message to UART */
ssize_t ret = ::write(_uart_fd, buf, packet_len);
if (ret != (int) packet_len) {
count_txerr();
count_txerrbytes(packet_len);
} else {
_last_write_success_time = _last_write_try_time;
count_txbytes(packet_len);
}
#endif
pthread_mutex_unlock(&_send_mutex);
}
void
Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID)
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
pthread_mutex_lock(&_send_mutex);
unsigned buf_free = get_free_tx_buf();
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
_last_write_try_time = hrt_absolute_time();
/* check if there is space in the buffer, let it overflow else */
if (buf_free < packet_len) {
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
pthread_mutex_unlock(&_send_mutex);
return;
}
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header */
buf[0] = MAVLINK_STX;
buf[1] = payload_len;
/* use mavlink's internal counter for the TX seq */
buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++;
buf[3] = mavlink_system.sysid;
buf[4] = (component_ID == 0) ? mavlink_system.compid : component_ID;
buf[5] = msgid;
/* payload */
memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len);
/* checksum */
uint16_t checksum;
crc_init(&checksum);
crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
crc_accumulate(mavlink_message_crcs[msgid], &checksum);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
/* send message to UART */
ssize_t ret = write(_uart_fd, buf, packet_len);
if (ret != (int) packet_len) {
count_txerr();
count_txerrbytes(packet_len);
} else {
_last_write_success_time = _last_write_try_time;
count_txbytes(packet_len);
}
pthread_mutex_unlock(&_send_mutex);
}
