//
// Returns integral $\int_{-1}^1 \psi_i(x) \psi_j(x) dx$.
// Integral is evaluated based on the Gaussian quadratures
//
double Lobatto::CalcMemi2(size_t i, size_t j) const
{
double w, x, v = 0;
assert(m_xGauss.size() == m_wGauss.size());
assert(m_xGauss.size() > 0);
for(size_t n = 0; n < m_xGauss.size(); n++)
{
x = m_xGauss[n];
w = m_wGauss[n];
v += w * Basis(i, x) * Basis(j, x);
}
return v;
}
Polygon3DEditor::Polygon3DEditor(EditorNode *p_editor) {
node = NULL;
editor = p_editor;
undo_redo = editor->get_undo_redo();
add_child(memnew(VSeparator));
button_create = memnew(ToolButton);
add_child(button_create);
button_create->connect("pressed", this, "_menu_option", varray(MODE_CREATE));
button_create->set_toggle_mode(true);
button_edit = memnew(ToolButton);
add_child(button_edit);
button_edit->connect("pressed", this, "_menu_option", varray(MODE_EDIT));
button_edit->set_toggle_mode(true);
mode = MODE_EDIT;
wip_active = false;
imgeom = memnew(ImmediateGeometry);
imgeom->set_transform(Transform(Basis(), Vector3(0, 0, 0.00001)));
line_material = Ref<SpatialMaterial>(memnew(SpatialMaterial));
line_material->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
line_material->set_line_width(3.0);
line_material->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true);
line_material->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
line_material->set_flag(SpatialMaterial::FLAG_SRGB_VERTEX_COLOR, true);
line_material->set_albedo(Color(1, 1, 1));
handle_material = Ref<SpatialMaterial>(memnew(SpatialMaterial));
handle_material->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
handle_material->set_flag(SpatialMaterial::FLAG_USE_POINT_SIZE, true);
handle_material->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true);
handle_material->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
handle_material->set_flag(SpatialMaterial::FLAG_SRGB_VERTEX_COLOR, true);
Ref<Texture> handle = editor->get_gui_base()->get_icon("Editor3DHandle", "EditorIcons");
handle_material->set_point_size(handle->get_width());
handle_material->set_texture(SpatialMaterial::TEXTURE_ALBEDO, handle);
pointsm = memnew(MeshInstance);
imgeom->add_child(pointsm);
m.instance();
pointsm->set_mesh(m);
pointsm->set_transform(Transform(Basis(), Vector3(0, 0, 0.00001)));
snap_ignore = false;
}
bool MobileVRInterface::initialize() {
ARVRServer *arvr_server = ARVRServer::get_singleton();
ERR_FAIL_NULL_V(arvr_server, false);
if (!initialized) {
// reset our sensor data and orientation
mag_count = 0;
has_gyro = false;
sensor_first = true;
mag_next_min = Vector3(10000, 10000, 10000);
mag_next_max = Vector3(-10000, -10000, -10000);
mag_current_min = Vector3(0, 0, 0);
mag_current_max = Vector3(0, 0, 0);
// reset our orientation
orientation = Basis();
// make this our primary interface
arvr_server->set_primary_interface(this);
last_ticks = OS::get_singleton()->get_ticks_usec();
;
initialized = true;
};
return true;
};
void ARVRServer::center_on_hmd(RotationMode p_rotation_mode, bool p_keep_height) {
if (primary_interface != NULL) {
// clear our current reference frame or we'll end up double adjusting it
reference_frame = Transform();
// requesting our EYE_MONO transform should return our current HMD position
Transform new_reference_frame = primary_interface->get_transform_for_eye(ARVRInterface::EYE_MONO, Transform());
// remove our tilt
if (p_rotation_mode == 1) {
// take the Y out of our Z
new_reference_frame.basis.set_axis(2, Vector3(new_reference_frame.basis.elements[0][2], 0.0, new_reference_frame.basis.elements[2][2]).normalized());
// Y is straight up
new_reference_frame.basis.set_axis(1, Vector3(0.0, 1.0, 0.0));
// and X is our cross reference
new_reference_frame.basis.set_axis(0, new_reference_frame.basis.get_axis(1).cross(new_reference_frame.basis.get_axis(2)).normalized());
} else if (p_rotation_mode == 2) {
// remove our rotation, we're only interesting in centering on position
new_reference_frame.basis = Basis();
};
// don't negate our height
if (p_keep_height) {
new_reference_frame.origin.y = 0.0;
};
reference_frame = new_reference_frame.inverse();
};
};
void GDAPI godot_basis_new_with_rows(godot_basis *r_dest, const godot_vector3 *p_x_axis, const godot_vector3 *p_y_axis, const godot_vector3 *p_z_axis) {
const Vector3 *x_axis = (const Vector3 *)p_x_axis;
const Vector3 *y_axis = (const Vector3 *)p_y_axis;
const Vector3 *z_axis = (const Vector3 *)p_z_axis;
Basis *dest = (Basis *)r_dest;
*dest = Basis(*x_axis, *y_axis, *z_axis);
}
/*
Adds a new parameter bind to connection.
*/
void ConnectDialog::_add_bind() {
if (cdbinds->params.size() >= VARIANT_ARG_MAX)
return;
Variant::Type vt = (Variant::Type)type_list->get_item_id(type_list->get_selected());
Variant value;
switch (vt) {
case Variant::BOOL: value = false; break;
case Variant::INT: value = 0; break;
case Variant::REAL: value = 0.0; break;
case Variant::STRING: value = ""; break;
case Variant::VECTOR2: value = Vector2(); break;
case Variant::RECT2: value = Rect2(); break;
case Variant::VECTOR3: value = Vector3(); break;
case Variant::PLANE: value = Plane(); break;
case Variant::QUAT: value = Quat(); break;
case Variant::AABB: value = AABB(); break;
case Variant::BASIS: value = Basis(); break;
case Variant::TRANSFORM: value = Transform(); break;
case Variant::COLOR: value = Color(); break;
default: { ERR_FAIL(); } break;
}
ERR_FAIL_COND(value.get_type() == Variant::NIL);
cdbinds->params.push_back(value);
cdbinds->notify_changed();
}
void SoftBody::_update_cache_pin_points_datas() {
if (pinned_points_cache_dirty) {
pinned_points_cache_dirty = false;
PoolVector<PinnedPoint>::Write w = pinned_points_indices.write();
for (int i = pinned_points_indices.size() - 1; 0 <= i; --i) {
if (!w[i].spatial_attachment_path.is_empty()) {
w[i].spatial_attachment = Object::cast_to<Spatial>(get_node(w[i].spatial_attachment_path));
if (w[i].spatial_attachment) {
Transform point_global_transform(get_global_transform());
point_global_transform.translate(PhysicsServer::get_singleton()->soft_body_get_point_offset(physics_rid, w[i].point_index));
// Local transform relative to spatial attachment node
w[i].vertex_offset_transform = w[i].spatial_attachment->get_global_transform().affine_inverse() * point_global_transform;
continue;
} else {
ERR_PRINTS("The node with path: " + String(w[i].spatial_attachment_path) + " was not found or is not a spatial node.");
}
}
// Local transform relative to Soft body
w[i].vertex_offset_transform.origin = PhysicsServer::get_singleton()->soft_body_get_point_offset(physics_rid, w[i].point_index);
w[i].vertex_offset_transform.basis = Basis();
}
}
}
Basis Basis::diagonalize() {
//NOTE: only implemented for symmetric matrices
//with the Jacobi iterative method method
ERR_FAIL_COND_V(!is_symmetric(), Basis());
const int ite_max = 1024;
real_t off_matrix_norm_2 = elements[0][1] * elements[0][1] + elements[0][2] * elements[0][2] + elements[1][2] * elements[1][2];
int ite = 0;
Basis acc_rot;
while (off_matrix_norm_2 > CMP_EPSILON2 && ite++ < ite_max ) {
real_t el01_2 = elements[0][1] * elements[0][1];
real_t el02_2 = elements[0][2] * elements[0][2];
real_t el12_2 = elements[1][2] * elements[1][2];
// Find the pivot element
int i, j;
if (el01_2 > el02_2) {
if (el12_2 > el01_2) {
i = 1;
j = 2;
} else {
i = 0;
j = 1;
}
} else {
if (el12_2 > el02_2) {
i = 1;
j = 2;
} else {
i = 0;
j = 2;
}
}
// Compute the rotation angle
real_t angle;
if (Math::abs(elements[j][j] - elements[i][i]) < CMP_EPSILON) {
angle = Math_PI / 4;
} else {
angle = 0.5 * Math::atan(2 * elements[i][j] / (elements[j][j] - elements[i][i]));
}
// Compute the rotation matrix
Basis rot;
rot.elements[i][i] = rot.elements[j][j] = Math::cos(angle);
rot.elements[i][j] = - (rot.elements[j][i] = Math::sin(angle));
// Update the off matrix norm
off_matrix_norm_2 -= elements[i][j] * elements[i][j];
// Apply the rotation
*this = rot * *this * rot.transposed();
acc_rot = rot * acc_rot;
}
return acc_rot;
}
//
// Returns the element $e[i][j]$ stiffness matrix element.
// The elements are read from precomputed array.
//
double Prob::CalcS(const Element& e, size_t i, size_t j) const
{
const double v1 = m_gamma * Mesi(i, j);
double v0 = 0, s, w;
if(m_g) // If function "g" is defined
{
for(size_t n = 0; n < m_xGauss.size(); n++)
{
s = m_xGauss[n];
w = m_wGauss[n];
v0 += w * Basis(i, s) * Basis(j, s) * m_g->Get(e.X(s));
}
}
const double jac = e.Jac();
return v1 / jac + v0 * jac;
}
MeshEditor::MeshEditor() {
viewport = memnew(Viewport);
Ref<World> world;
world.instance();
viewport->set_world(world); //use own world
add_child(viewport);
viewport->set_disable_input(true);
set_stretch(true);
camera = memnew(Camera);
camera->set_transform(Transform(Basis(), Vector3(0, 0, 1.1)));
camera->set_perspective(45, 0.1, 10);
viewport->add_child(camera);
light1 = memnew(DirectionalLight);
light1->set_transform(Transform().looking_at(Vector3(-1, -1, -1), Vector3(0, 1, 0)));
viewport->add_child(light1);
light2 = memnew(DirectionalLight);
light2->set_transform(Transform().looking_at(Vector3(0, 1, 0), Vector3(0, 0, 1)));
light2->set_color(Color(0.7, 0.7, 0.7));
viewport->add_child(light2);
rotation = memnew(Spatial);
viewport->add_child(rotation);
mesh_instance = memnew(MeshInstance);
rotation->add_child(mesh_instance);
set_custom_minimum_size(Size2(1, 150) * EDSCALE);
HBoxContainer *hb = memnew(HBoxContainer);
add_child(hb);
hb->set_anchors_and_margins_preset(Control::PRESET_WIDE, Control::PRESET_MODE_MINSIZE, 2);
hb->add_spacer();
VBoxContainer *vb_light = memnew(VBoxContainer);
hb->add_child(vb_light);
light_1_switch = memnew(TextureButton);
light_1_switch->set_toggle_mode(true);
vb_light->add_child(light_1_switch);
light_1_switch->connect("pressed", this, "_button_pressed", varray(light_1_switch));
light_2_switch = memnew(TextureButton);
light_2_switch->set_toggle_mode(true);
vb_light->add_child(light_2_switch);
light_2_switch->connect("pressed", this, "_button_pressed", varray(light_2_switch));
first_enter = true;
rot_x = 0;
rot_y = 0;
}
double Basis(int m, int M, float x, float xmin, float DX){
double y = 0;
double xm = xmin + (m * DX);
double z = abs((double)(x - xm) / (double)DX);
if (z < 2.0) {
z = 2 - z;
y = 0.25 * (z*z*z);
z -= 1.0;
if (z > 0)
y -= (z*z*z);
}
// Boundary conditions, if any, are an additional addend.
if (m == 0 || m == 1)
y += Beta(m, M) * Basis(-1, M, x, xmin, DX);
else if (m == M-1 || m == M)
y += Beta(m, M) * Basis(M+1, M, x, xmin, DX);
return y;
}
float AdvecInit(){
int i, p, j,k;
float c, midpoint, X_Points;
double answer;
midpoint = IEL_SIZE/2.0;
for (i=1; i<IELEM; i++) {
for (k=0; k<IFLOWNUM; k++) {
for (p=0; p<IORDER; p++) {
sol[i][p][k] = 0.0;
for (j=0; j<6; j++) {
X_Points = midpoint + 0.5*IEL_SIZE*rXg[j];
sol[i][p][k] = sol[i][p][k] + InitCond((double)X_Points)*Basis(p,rXg[j])*rWg[j];
} // quad
sol[i][p][k] = sol[i][p][k]/Norms(p);
}// order
} // flow component
midpoint = midpoint + IEL_SIZE;
}// element
FILE *fpp = fopen("Init.dat","w");
for (i=0 ; i<INODE-1; i++){
fprintf(fpp,"%d %f\n",i,sol[i][0][0] );
//i++;
}
fclose(fpp);
}
float evaluate(float x, float * coeficients, float xmin, float DX, int M, float mean) {
float y = 0;
if (true) {
int n = (int)((x - xmin)/DX);
// M ?
// s-A[] ?
for (int i = max(0, n-1); i <= min(M, n+2); ++i) {
y += coeficients[i] * Basis(i, M, x, xmin, DX);
}
y += mean;
}
return y;
}
//
// Returns the element $b[i]$ of load matrix element.
// Gauss quadrature applied.
//
double Prob::CalcB(const Element& e, size_t i) const
{
double w, s, b = 0;
assert(m_f);
assert(m_xGauss.size() == m_wGauss.size());
for(size_t n = 0; n < m_xGauss.size(); n++)
{
s = m_xGauss[n];
w = m_wGauss[n];
b += w * Basis(i, s) * m_f->Get(e.X(s));
}
return e.Jac() * b;
}
// Decomposes a Basis into a rotation-reflection matrix (an element of the group O(3)) and a positive scaling matrix as B = O.S.
// Returns the rotation-reflection matrix via reference argument, and scaling information is returned as a Vector3.
// This (internal) function is too specific and named too ugly to expose to users, and probably there's no need to do so.
Vector3 Basis::rotref_posscale_decomposition(Basis &rotref) const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V(determinant() == 0, Vector3());
Basis m = transposed() * (*this);
ERR_FAIL_COND_V(!m.is_diagonal(), Vector3());
#endif
Vector3 scale = get_scale();
Basis inv_scale = Basis().scaled(scale.inverse()); // this will also absorb the sign of scale
rotref = (*this) * inv_scale;
#ifdef MATH_CHECKS
ERR_FAIL_COND_V(!rotref.is_orthogonal(), Vector3());
#endif
return scale.abs();
}
void GridMapEditor::update_grid() {
grid_xform.origin.x-=1; //force update in hackish way.. what do i care
//VS *vs = VS::get_singleton();
grid_ofs[edit_axis]=edit_floor[edit_axis]*node->get_cell_size();
edit_grid_xform.origin=grid_ofs;
edit_grid_xform.basis=Basis();
for(int i=0;i<3;i++) {
VisualServer::get_singleton()->instance_geometry_set_flag(grid_instance[i],VS::INSTANCE_FLAG_VISIBLE,i==edit_axis);
}
updating=true;
floor->set_value(edit_floor[edit_axis]);
updating=false;
}
/* Bezier curve subroutine */
void
bezier(int npts, SUMOReal b[], int cpts, SUMOReal p[]) {
int i;
int j;
int i1;
int icount;
int jcount;
SUMOReal step;
SUMOReal t;
SUMOReal factrl(int);
SUMOReal Ni(int,int);
SUMOReal Basis(int,int,SUMOReal);
/* calculate the points on the Bezier curve */
icount = 0;
t = 0;
step = (SUMOReal) 1.0/(cpts -1);
for (i1 = 1; i1<=cpts; i1++) { /* main loop */
if ((1.0 - t) < 5e-6) t = 1.0;
for (j = 1; j <= 3; j++) { /* generate a point on the curve */
jcount = j;
p[icount+j] = 0.;
for (i = 1; i <= npts; i++) { /* Do x,y,z components */
p[icount + j] = p[icount + j] + Basis(npts-1,i-1,t)*b[jcount];
jcount = jcount + 3;
}
}
icount = icount + 3;
t = t + step;
}
}
Basis Basis::rotated_local(const Vector3 &p_axis, real_t p_phi) const {
return (*this) * Basis(p_axis, p_phi);
}
void GDAPI godot_basis_new_with_euler_quat(godot_basis *r_dest, const godot_quat *p_euler) {
Basis *dest = (Basis *)r_dest;
const Quat *euler = (const Quat *)p_euler;
*dest = Basis(*euler);
}
void GDAPI godot_basis_new(godot_basis *r_dest) {
Basis *dest = (Basis *)r_dest;
*dest = Basis();
}
Basis Basis::rotated(const Quat &p_quat) const {
return Basis(p_quat) * (*this);
}
void ProceduralSky::_update_sky() {
update_queued = false;
PoolVector<uint8_t> imgdata;
static const int size[TEXTURE_SIZE_MAX] = {
1024, 2048, 4096
};
int w = size[texture_size];
int h = w / 2;
imgdata.resize(w * h * 4); //RGBE
{
PoolVector<uint8_t>::Write dataw = imgdata.write();
uint32_t *ptr = (uint32_t *)dataw.ptr();
Color sky_top_linear = sky_top_color.to_linear();
Color sky_horizon_linear = sky_horizon_color.to_linear();
Color ground_bottom_linear = ground_bottom_color.to_linear();
Color ground_horizon_linear = ground_horizon_color.to_linear();
//Color sun_linear = sun_color.to_linear();
Vector3 sun(0, 0, -1);
sun = Basis(Vector3(1, 0, 0), Math::deg2rad(sun_latitude)).xform(sun);
sun = Basis(Vector3(0, 1, 0), Math::deg2rad(sun_longitude)).xform(sun);
sun.normalize();
for (int i = 0; i < w; i++) {
float u = float(i) / (w - 1);
float phi = u * 2.0 * Math_PI;
for (int j = 0; j < h; j++) {
float v = float(j) / (h - 1);
float theta = v * Math_PI;
Vector3 normal(
Math::sin(phi) * Math::sin(theta) * -1.0,
Math::cos(theta),
Math::cos(phi) * Math::sin(theta) * -1.0);
normal.normalize();
float v_angle = Math::acos(normal.y);
Color color;
if (normal.y < 0) {
//ground
float c = (v_angle - (Math_PI * 0.5)) / (Math_PI * 0.5);
color = ground_horizon_linear.linear_interpolate(ground_bottom_linear, Math::ease(c, ground_curve));
} else {
float c = v_angle / (Math_PI * 0.5);
color = sky_horizon_linear.linear_interpolate(sky_top_linear, Math::ease(1.0 - c, sky_curve));
float sun_angle = Math::rad2deg(Math::acos(sun.dot(normal)));
if (sun_angle < sun_angle_min) {
color = color.blend(sun_color);
} else if (sun_angle < sun_angle_max) {
float c2 = (sun_angle - sun_angle_min) / (sun_angle_max - sun_angle_min);
c2 = Math::ease(c2, sun_curve);
color = color.blend(sun_color).linear_interpolate(color, c2);
}
}
ptr[j * w + i] = color.to_rgbe9995();
}
}
}
Ref<Image> image;
image.instance();
image->create(w, h, false, Image::FORMAT_RGBE9995, imgdata);
VS::get_singleton()->texture_allocate(texture, w, h, Image::FORMAT_RGBE9995, VS::TEXTURE_FLAG_FILTER | VS::TEXTURE_FLAG_REPEAT);
VS::get_singleton()->texture_set_data(texture, image);
_radiance_changed();
}
real_t s = Math::sqrt(elements[i][i] - elements[j][j] - elements[k][k] + 1.0);
temp[i] = s * 0.5;
s = 0.5 / s;
temp[3] = (elements[k][j] - elements[j][k]) * s;
temp[j] = (elements[j][i] + elements[i][j]) * s;
temp[k] = (elements[k][i] + elements[i][k]) * s;
}
return Quat(temp[0],temp[1],temp[2],temp[3]);
}
static const Basis _ortho_bases[24]={
Basis(1, 0, 0, 0, 1, 0, 0, 0, 1),
Basis(0, -1, 0, 1, 0, 0, 0, 0, 1),
Basis(-1, 0, 0, 0, -1, 0, 0, 0, 1),
Basis(0, 1, 0, -1, 0, 0, 0, 0, 1),
Basis(1, 0, 0, 0, 0, -1, 0, 1, 0),
Basis(0, 0, 1, 1, 0, 0, 0, 1, 0),
Basis(-1, 0, 0, 0, 0, 1, 0, 1, 0),
Basis(0, 0, -1, -1, 0, 0, 0, 1, 0),
Basis(1, 0, 0, 0, -1, 0, 0, 0, -1),
Basis(0, 1, 0, 1, 0, 0, 0, 0, -1),
Basis(-1, 0, 0, 0, 1, 0, 0, 0, -1),
Basis(0, -1, 0, -1, 0, 0, 0, 0, -1),
Basis(1, 0, 0, 0, 0, 1, 0, -1, 0),
Basis(0, 0, -1, 1, 0, 0, 0, -1, 0),
Basis(-1, 0, 0, 0, 0, -1, 0, -1, 0),
Basis(0, 0, 1, -1, 0, 0, 0, -1, 0),
void MobileVRInterface::set_position_from_sensors() {
_THREAD_SAFE_METHOD_
// this is a helper function that attempts to adjust our transform using our 9dof sensors
// 9dof is a misleading marketing term coming from 3 accelerometer axis + 3 gyro axis + 3 magnetometer axis = 9 axis
// but in reality this only offers 3 dof (yaw, pitch, roll) orientation
uint64_t ticks = OS::get_singleton()->get_ticks_usec();
uint64_t ticks_elapsed = ticks - last_ticks;
float delta_time = (double)ticks_elapsed / 1000000.0;
// few things we need
Input *input = Input::get_singleton();
Vector3 down(0.0, -1.0, 0.0); // Down is Y negative
Vector3 north(0.0, 0.0, 1.0); // North is Z positive
// make copies of our inputs
bool has_grav = false;
Vector3 acc = input->get_accelerometer();
Vector3 gyro = input->get_gyroscope();
Vector3 grav = input->get_gravity();
Vector3 magneto = scale_magneto(input->get_magnetometer()); // this may be overkill on iOS because we're already getting a calibrated magnetometer reading
if (sensor_first) {
sensor_first = false;
} else {
acc = scrub(acc, last_accerometer_data, 2, 0.2);
magneto = scrub(magneto, last_magnetometer_data, 3, 0.3);
};
last_accerometer_data = acc;
last_magnetometer_data = magneto;
if (grav.length() < 0.1) {
// not ideal but use our accelerometer, this will contain shakey shakey user behaviour
// maybe look into some math but I'm guessing that if this isn't available, its because we lack the gyro sensor to actually work out
// what a stable gravity vector is
grav = acc;
if (grav.length() > 0.1) {
has_grav = true;
};
} else {
has_grav = true;
};
bool has_magneto = magneto.length() > 0.1;
if (gyro.length() > 0.1) {
/* this can return to 0.0 if the user doesn't move the phone, so once on, it's on */
has_gyro = true;
};
if (has_gyro) {
// start with applying our gyro (do NOT smooth our gyro!)
Basis rotate;
rotate.rotate(orientation.get_axis(0), gyro.x * delta_time);
rotate.rotate(orientation.get_axis(1), gyro.y * delta_time);
rotate.rotate(orientation.get_axis(2), gyro.z * delta_time);
orientation = rotate * orientation;
tracking_state = ARVRInterface::ARVR_NORMAL_TRACKING;
};
///@TODO improve this, the magnetometer is very fidgity sometimes flipping the axis for no apparent reason (probably a bug on my part)
// if you have a gyro + accelerometer that combo tends to be better then combining all three but without a gyro you need the magnetometer..
if (has_magneto && has_grav && !has_gyro) {
// convert to quaternions, easier to smooth those out
Quat transform_quat(orientation);
Quat acc_mag_quat(combine_acc_mag(grav, magneto));
transform_quat = transform_quat.slerp(acc_mag_quat, 0.1);
orientation = Basis(transform_quat);
tracking_state = ARVRInterface::ARVR_NORMAL_TRACKING;
} else if (has_grav) {
// use gravity vector to make sure down is down...
// transform gravity into our world space
grav.normalize();
Vector3 grav_adj = orientation.xform(grav);
float dot = grav_adj.dot(down);
if ((dot > -1.0) && (dot < 1.0)) {
// axis around which we have this rotation
Vector3 axis = grav_adj.cross(down);
axis.normalize();
Basis drift_compensation(axis, acos(dot) * delta_time * 10);
orientation = drift_compensation * orientation;
};
};
// JIC
orientation.orthonormalize();
last_ticks = ticks;
};
void GDAPI godot_basis_new_with_axis_and_angle(godot_basis *r_dest, const godot_vector3 *p_axis, const godot_real p_phi) {
const Vector3 *axis = (const Vector3 *)p_axis;
Basis *dest = (Basis *)r_dest;
*dest = Basis(*axis, p_phi);
}
bool GridMapEditor::do_input_action(Camera* p_camera,const Point2& p_point,bool p_click) {
if (!spatial_editor)
return false;
if (selected_pallete<0 && input_action!=INPUT_COPY && input_action!=INPUT_SELECT && input_action!=INPUT_DUPLICATE)
return false;
Ref<MeshLibrary> theme = node->get_theme();
if (theme.is_null())
return false;
if (input_action!=INPUT_COPY && input_action!=INPUT_SELECT && input_action!=INPUT_DUPLICATE && !theme->has_item(selected_pallete))
return false;
Camera *camera = p_camera;
Vector3 from = camera->project_ray_origin(p_point);
Vector3 normal = camera->project_ray_normal(p_point);
Transform local_xform = node->get_global_transform().affine_inverse();
Vector<Plane> planes=camera->get_frustum();
from=local_xform.xform(from);
normal=local_xform.basis.xform(normal).normalized();
Plane p;
p.normal[edit_axis]=1.0;
p.d=edit_floor[edit_axis]*node->get_cell_size();
Vector3 inters;
if (!p.intersects_segment(from, from + normal * settings_pick_distance->get_value(), &inters))
return false;
//make sure the intersection is inside the frustum planes, to avoid
//painting on invisible regions
for(int i=0;i<planes.size();i++) {
Plane fp = local_xform.xform(planes[i]);
if (fp.is_point_over(inters))
return false;
}
int cell[3];
float cell_size[3]={node->get_cell_size(),node->get_cell_size(),node->get_cell_size()};
last_mouseover=Vector3(-1,-1,-1);
for(int i=0;i<3;i++) {
if (i==edit_axis)
cell[i]=edit_floor[i];
else {
cell[i]=inters[i]/node->get_cell_size();
if (inters[i]<0)
cell[i]-=1; //compensate negative
grid_ofs[i]=cell[i]*cell_size[i];
}
/*if (cell[i]<0 || cell[i]>=grid_size[i]) {
cursor_visible=false;
_update_cursor_transform();
return false;
}*/
}
last_mouseover=Vector3(cell[0],cell[1],cell[2]);
VS::get_singleton()->instance_set_transform(grid_instance[edit_axis],Transform(Basis(),grid_ofs));
if (cursor_instance.is_valid()) {
cursor_origin=(Vector3(cell[0],cell[1],cell[2])+Vector3(0.5*node->get_center_x(),0.5*node->get_center_y(),0.5*node->get_center_z()))*node->get_cell_size();
cursor_visible=true;
_update_cursor_transform();
}
if (input_action==INPUT_DUPLICATE) {
selection.current=Vector3(cell[0],cell[1],cell[2]);
_update_duplicate_indicator();
} else if (input_action==INPUT_SELECT) {
selection.current=Vector3(cell[0],cell[1],cell[2]);
if (p_click)
selection.click=selection.current;
selection.active=true;
_validate_selection();
return true;
} else if (input_action==INPUT_COPY) {
int item=node->get_cell_item(cell[0],cell[1],cell[2]);
if (item>=0) {
selected_pallete=item;
theme_pallete->set_current(item);
update_pallete();
_update_cursor_instance();
}
return true;
} if (input_action==INPUT_PAINT) {
SetItem si;
si.pos=Vector3(cell[0],cell[1],cell[2]);
si.new_value=selected_pallete;
si.new_orientation=cursor_rot;
si.old_value=node->get_cell_item(cell[0],cell[1],cell[2]);
si.old_orientation=node->get_cell_item_orientation(cell[0],cell[1],cell[2]);
set_items.push_back(si);
node->set_cell_item(cell[0],cell[1],cell[2],selected_pallete,cursor_rot);
return true;
} else if (input_action==INPUT_ERASE) {
SetItem si;
si.pos=Vector3(cell[0],cell[1],cell[2]);
si.new_value=-1;
si.new_orientation=0;
si.old_value=node->get_cell_item(cell[0],cell[1],cell[2]);
si.old_orientation=node->get_cell_item_orientation(cell[0],cell[1],cell[2]);
set_items.push_back(si);
node->set_cell_item(cell[0],cell[1],cell[2],-1);
return true;
}
return false;
}
Basis Basis::rotated(const Vector3& p_euler) const {
return Basis(p_euler) * (*this);
}
// Multiplies the matrix from left by the rotation matrix: M -> R.M
// Note that this does *not* rotate the matrix itself.
//
// The main use of Basis is as Transform.basis, which is used a the transformation matrix
// of 3D object. Rotate here refers to rotation of the object (which is R * (*this)),
// not the matrix itself (which is R * (*this) * R.transposed()).
Basis Basis::rotated(const Vector3& p_axis, real_t p_phi) const {
return Basis(p_axis, p_phi) * (*this);
}
void GDAPI godot_basis_new_with_euler(godot_basis *r_dest, const godot_vector3 *p_euler) {
const Vector3 *euler = (const Vector3 *)p_euler;
Basis *dest = (Basis *)r_dest;
*dest = Basis(*euler);
}
Basis BulletPhysicsDirectBodyState::get_principal_inertia_axes() const {
return Basis();
}
