From fb6ca6a160d7eb10534de78096eb941fb75a6601 Mon Sep 17 00:00:00 2001 From: cxgeorge <> Date: Thu, 24 May 2001 02:55:11 +0000 Subject: [PATCH] many micro-optimizations --- panda/src/audio/audio_win_traits.cxx | 2 +- panda/src/collide/collisionPlane.cxx | 2 +- panda/src/dxgsg/dxGraphicsStateGuardian.cxx | 77 +++--- panda/src/express/referenceCount.I | 4 + panda/src/framework/framework.cxx | 10 - panda/src/glgsg/glGraphicsStateGuardian.cxx | 72 ++--- panda/src/linmath/compose_matrix_src.I | 4 +- panda/src/linmath/compose_matrix_src.cxx | 285 ++++++++++---------- panda/src/linmath/dblnames.h | 3 + panda/src/linmath/deg_2_rad.h | 7 +- panda/src/linmath/fltnames.h | 2 + panda/src/linmath/lmatrix3_src.I | 110 +++++--- panda/src/linmath/lmatrix3_src.cxx | 6 +- panda/src/linmath/lmatrix3_src.h | 9 + panda/src/linmath/lmatrix4_src.I | 204 ++++++++------ panda/src/linmath/lmatrix4_src.cxx | 50 ++-- panda/src/linmath/lmatrix4_src.h | 14 +- panda/src/linmath/lorientation_src.I | 4 +- panda/src/linmath/lpoint3_src.I | 2 +- panda/src/linmath/lquaternion_src.I | 6 +- panda/src/linmath/lquaternion_src.cxx | 2 +- panda/src/linmath/lrotation_src.I | 4 +- panda/src/linmath/luse.h | 3 + panda/src/linmath/lvecBase2_src.I | 4 +- panda/src/linmath/lvecBase2_src.cxx | 6 +- panda/src/linmath/lvecBase3_src.I | 9 +- panda/src/linmath/lvecBase3_src.cxx | 8 +- panda/src/linmath/lvecBase4_src.I | 4 +- panda/src/linmath/lvecBase4_src.cxx | 10 +- panda/src/linmath/lvector2_src.I | 8 +- panda/src/linmath/lvector3_src.I | 28 +- panda/src/linmath/lvector4_src.I | 8 +- panda/src/linmath/mathNumbers.cxx | 7 +- panda/src/linmath/mathNumbers.h | 3 + panda/src/linmath/test_math.cxx | 10 +- 35 files changed, 557 insertions(+), 430 deletions(-) diff --git a/panda/src/audio/audio_win_traits.cxx b/panda/src/audio/audio_win_traits.cxx index fc13e99e57..232f004c17 100644 --- a/panda/src/audio/audio_win_traits.cxx +++ b/panda/src/audio/audio_win_traits.cxx @@ -851,7 +851,7 @@ void WinSamplePlayer::play_sound(AudioTraits::SoundClass* sample, chan->Stop(); DWORD l = wsample->get_length(); WAVEFORMATEX f = wsample->get_format(); - float factor = ((float)l) / wsample->get_format().nAvgBytesPerSec; + float factor = ((float)l) / f.nAvgBytesPerSec; factor = start_time / factor; if (factor > 1.) factor = 1.; diff --git a/panda/src/collide/collisionPlane.cxx b/panda/src/collide/collisionPlane.cxx index 801a5a3d30..ff160a20a8 100644 --- a/panda/src/collide/collisionPlane.cxx +++ b/panda/src/collide/collisionPlane.cxx @@ -225,7 +225,7 @@ recompute_viz(Node *parent) { p1 = LPoint3f(1.0, 1.0, -(normal[0] + normal[1] + D)/normal[2]) - cp; } - p1 = normalize(p1); + p1.normalize(); p2 = cross(normal, p1); p3 = cross(normal, p2); p4 = cross(normal, p3); diff --git a/panda/src/dxgsg/dxGraphicsStateGuardian.cxx b/panda/src/dxgsg/dxGraphicsStateGuardian.cxx index 238cf5db3d..cbb61f0685 100644 --- a/panda/src/dxgsg/dxGraphicsStateGuardian.cxx +++ b/panda/src/dxgsg/dxGraphicsStateGuardian.cxx @@ -198,18 +198,18 @@ reset() { // Set up our clear values to invalid values, so the glClear* calls // will be made initially. - _clear_color_red = -1.0; - _clear_color_green = -1.0; - _clear_color_blue = -1.0; - _clear_color_alpha = -1.0; - _clear_depth = -1.0; + _clear_color_red = -1.0f; + _clear_color_green = -1.0f; + _clear_color_blue = -1.0f; + _clear_color_alpha = -1.0f; + _clear_depth = -1.0f; _clear_stencil = -1; - _clear_accum_red = -1.0; - _clear_accum_green = -1.0; - _clear_accum_blue = -1.0; - _clear_accum_alpha = -1.0; - _line_width = 1.0; - _point_size = 1.0; + _clear_accum_red = -1.0f; + _clear_accum_green = -1.0f; + _clear_accum_blue = -1.0f; + _clear_accum_alpha = -1.0f; + _line_width = 1.0f; + _point_size = 1.0f; _depth_mask = false; _fog_mode = D3DFOG_EXP; _alpha_func = D3DCMP_ALWAYS; @@ -274,7 +274,7 @@ init_dx( LPDIRECTDRAW7 context, if(dx_show_fps_meter) { _start_time = timeGetTime(); - _current_fps = 0.0; + _current_fps = 0.0f; _start_frame_count = _cur_frame_count = 0; } @@ -465,7 +465,7 @@ init_dx( LPDIRECTDRAW7 context, #ifdef _DEBUG if ((_D3DDevDesc.dpcTriCaps.dwRasterCaps & D3DPRASTERCAPS_MIPMAPLODBIAS) && - (dx_global_miplevel_bias!=0.0)) { + (dx_global_miplevel_bias!=0.0f)) { _d3dDevice->SetTextureStageState(0, D3DTSS_MIPMAPLODBIAS, *((LPDWORD) (&dx_global_miplevel_bias)) ); } #endif @@ -508,7 +508,7 @@ clear(const RenderBuffer &buffer) { _color_clear_value[0] = .5; /* The following lines will cause the background color to cycle from black to red. _color_clear_value[0] += .001; - if (_color_clear_value[0] > 1.0) _color_clear_value[0] = 0.0; + if (_color_clear_value[0] > 1.0f) _color_clear_value[0] = 0.0f; */ } @@ -938,7 +938,7 @@ render_subgraph(RenderTraverser *traverser, dxgsg_cat.info() << "cur projection matrix: " << projection_mat << endl; } - // note: a projection matrix that does not have a [3][4] value of 1.0 is + // note: a projection matrix that does not have a [3][4] value of 1.0f is // not w-compliant and could cause problems with fog } @@ -1225,7 +1225,7 @@ draw_point(const GeomPoint *geom) { if (!bPrintedMsg && (geom->get_size()!=1.0f)) { bPrintedMsg=TRUE; - dxgsg_cat.warning() << "D3D does not support drawing points of non-unit size, setting point size to 1.0!\n"; + dxgsg_cat.warning() << "D3D does not support drawing points of non-unit size, setting point size to 1.0f!\n"; } #endif @@ -1360,7 +1360,7 @@ draw_line(const GeomLine* geom) { if (!bPrintedMsg && (geom->get_width()!=1.0f)) { bPrintedMsg=TRUE; - dxgsg_cat.warning() << "DX does not support drawing lines with a non-1.0 pixel width, setting width to 1.0!\n"; + dxgsg_cat.warning() << "DX does not support drawing lines with a non-1.0f pixel width, setting width to 1.0f!\n"; } #endif @@ -1471,7 +1471,7 @@ draw_linestrip(const GeomLinestrip* geom) { if (!bPrintedMsg && (geom->get_width()!=1.0f)) { bPrintedMsg=TRUE; - dxgsg_cat.warning() << "DX does not support drawing lines with a non-1.0 pixel width, setting width to 1.0!\n"; + dxgsg_cat.warning() << "DX does not support drawing lines with a non-1.0f pixel width, setting width to 1.0f!\n"; } #endif @@ -1843,7 +1843,7 @@ draw_sprite(const GeomSprite *geom) { #endif _pCurFvfBufPtr = _pFvfBufBasePtr; // _pCurFvfBufPtr changes, _pFvfBufBasePtr doesn't - const float TexCrdSets[4][2] = {{0.0,0.0},{1.0,0.0},{0.0,1.0},{1.0,1.0}}; + const float TexCrdSets[4][2] = {{0.0f,0.0f},{1.0f,0.0f},{0.0f,1.0f},{1.0f,1.0f}}; #define QUADVERTLISTLEN 6 @@ -1871,13 +1871,14 @@ draw_sprite(const GeomSprite *geom) { // create the rotated points. BUGBUG: this matmult will be slow if we dont get inlining // rotate_mat calls sin() on an unbounded val, possible to make it faster with lookup table (modulate to 0-360 range?) - LMatrix3f xform_mat = LMatrix3f::rotate_mat(theta) * + + LMatrix3f xform_mat = LMatrix3f::rotate_mat(theta) * LMatrix3f::scale_mat(scaled_width, scaled_height); - ur = (LVector3f( 1, 1, 0) * xform_mat) + pSpr->_v; - ul = (LVector3f(-1, 1, 0) * xform_mat) + pSpr->_v; - lr = (LVector3f( 1, -1, 0) * xform_mat) + pSpr->_v; - ll = (LVector3f(-1, -1, 0) * xform_mat) + pSpr->_v; + ur = (LVector3f( 1.0f, 1.0f, 0.0f) * xform_mat) + pSpr->_v; + ul = (LVector3f(-1.0f, 1.0f, 0.0f) * xform_mat) + pSpr->_v; + lr = (LVector3f( 1.0f, -1.0f, 0.0f) * xform_mat) + pSpr->_v; + ll = (LVector3f(-1.0f, -1.0f, 0.0f) * xform_mat) + pSpr->_v; } else { // create points for unrotated rect sprites float x,y,negx,negy,z; @@ -2301,9 +2302,9 @@ draw_tri(const GeomTri *geom) { #if 0 // test triangle for me to dbg experiments only float vert_buf[15] = { - 0.0, 0.0, 0.0, 0.0, 0.0, - 33.0, 0.0, 0.0, 0.0, 2.0, - 0.0, 0.0, 33.0, 2.0, 0.0 + 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, + 33.0, 0.0f, 0.0f, 0.0f, 2.0, + 0.0f, 0.0f, 33.0, 2.0, 0.0f }; _d3dDevice->SetTextureStageState(0,D3DTSS_ADDRESSU,D3DTADDRESS_BORDER); @@ -2874,7 +2875,7 @@ GenerateSphere(void *pVertexSpace,DWORD dwVertSpaceByteSize, if (DOTEXTURING) { // numRings already includes 1st and last rings for this case dtheta = (float)(M_PI / (wNumRings-1)); //Angle between each ring (ignore 2 fake rings) - theta = 0.0; + theta = 0.0f; } else { dtheta = (float)(M_PI / (wNumRings + 1)); //Angle between each ring theta = dtheta; @@ -2883,7 +2884,7 @@ GenerateSphere(void *pVertexSpace,DWORD dwVertSpaceByteSize, for (i = 0; i < wNumRings; i++) { float costheta,sintheta,cosphi,sinphi; - phi = 0.0; + phi = 0.0f; if (DOTEXTURING) { texCoords[1] = theta * reciprocal_PI; // v is the same for each ring @@ -2917,7 +2918,7 @@ GenerateSphere(void *pVertexSpace,DWORD dwVertSpaceByteSize, add_DWORD_to_FVFBuf(p_colr); if (DOTEXTURING) { - texCoords[0] = 1.0 - phi*reciprocal_2PI; + texCoords[0] = 1.0f - phi*reciprocal_2PI; add_to_FVFBuf((void *)texCoords, sizeof(TexCoordf)); } @@ -3112,7 +3113,7 @@ issue_alpha_transform(const AlphaTransformAttribute *attrib) { _current_alpha_offset= attrib->get_offset(); _current_alpha_scale = attrib->get_scale(); - if ((_current_alpha_offset == 0.0) && (_current_alpha_scale == 1.0)) { + if ((_current_alpha_offset == 0.0f) && (_current_alpha_scale == 1.0f)) { _alpha_transform_enabled = false; } else { _alpha_transform_enabled = true; @@ -3830,7 +3831,7 @@ apply_fog(Fog *fog) { Colorf fog_colr = fog->get_color(); _d3dDevice->SetRenderState(D3DRENDERSTATE_FOGCOLOR, - D3DRGBA(fog_colr[0], fog_colr[1], fog_colr[2], 0.0)); + D3DRGBA(fog_colr[0], fog_colr[1], fog_colr[2], 0.0f)); } //////////////////////////////////////////////////////////////////// @@ -4117,12 +4118,12 @@ issue_transform(const TransformAttribute *attrib) { } VERTFORMAT; VERTFORMAT vert_buf[] = { - {0.0, 0.0, 0.0, 0.0, -1.0, 0.0, D3DRGBA(1.0, 0.0, 0.0, 1.0)}, // red - {3.0, 0.0, 0.0, 0.0, -1.0, 0.0, D3DRGBA(1.0, 0.0, 0.0, 1.0)}, // red - {0.0, 0.0, 0.0, 0.0, -1.0, 0.0, D3DRGBA(0.0, 1.0, 0.0, 1.0)}, // grn - {0.0, 3.0, 0.0, 0.0, -1.0, 0.0, D3DRGBA(0.0, 1.0, 0.0, 1.0)}, // grn - {0.0, 0.0, 0.0, 0.0, -1.0, 0.0, D3DRGBA(0.0, 0.0, 1.0, 1.0)}, // blu - {0.0, 0.0, 3.0, 0.0, -1.0, 0.0, D3DRGBA(0.0, 0.0, 1.0, 1.0)}, // blu + {0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, D3DRGBA(1.0f, 0.0f, 0.0f, 1.0f)}, // red + {3.0, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, D3DRGBA(1.0f, 0.0f, 0.0f, 1.0f)}, // red + {0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, D3DRGBA(0.0f, 1.0f, 0.0f, 1.0f)}, // grn + {0.0f, 3.0, 0.0f, 0.0f, -1.0f, 0.0f, D3DRGBA(0.0f, 1.0f, 0.0f, 1.0f)}, // grn + {0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, D3DRGBA(0.0f, 0.0f, 1.0f, 1.0f)}, // blu + {0.0f, 0.0f, 3.0, 0.0f, -1.0f, 0.0f, D3DRGBA(0.0f, 0.0f, 1.0f, 1.0f)}, // blu }; HRESULT hr = _d3dDevice->DrawPrimitive(D3DPT_LINELIST, D3DFVF_DIFFUSE | D3DFVF_XYZ | D3DFVF_NORMAL, diff --git a/panda/src/express/referenceCount.I b/panda/src/express/referenceCount.I index da0e4106de..69c3dc9975 100644 --- a/panda/src/express/referenceCount.I +++ b/panda/src/express/referenceCount.I @@ -150,7 +150,9 @@ unref_consider_delete() { //////////////////////////////////////////////////////////////////// INLINE int ReferenceCount:: get_ref_count() const { + #ifndef NDEBUG test_ref_count_integrity(); + #endif return _ref_count; } @@ -259,6 +261,7 @@ unref() const { //////////////////////////////////////////////////////////////////// INLINE void ReferenceCount:: test_ref_count_integrity() const { + #ifndef NDEBUG nassertv(this != NULL); // If this assertion fails, we're trying to access a pointer that @@ -273,6 +276,7 @@ test_ref_count_integrity() const { // up altogether. Maybe some errant code stomped all over memory // somewhere. nassertv(_ref_count >= 0); + #endif } //////////////////////////////////////////////////////////////////// diff --git a/panda/src/framework/framework.cxx b/panda/src/framework/framework.cxx index 7f55230827..0f776c7262 100644 --- a/panda/src/framework/framework.cxx +++ b/panda/src/framework/framework.cxx @@ -1009,16 +1009,6 @@ static void move_gridded_stuff(GriddedMotionType gridmotiontype,gridded_file_inf xpos = InfoArr[i].xstart = InfoArr[i].xend; ypos = InfoArr[i].ystart = InfoArr[i].yend; -/* - if(InfoArr[i].xend == -wander_area_pos_offset) { - InfoArr[i].xend = wander_area_pos_offset; - InfoArr[i].yend = wander_area_pos_offset; - } else { - InfoArr[i].xend = -wander_area_pos_offset; - InfoArr[i].yend = -wander_area_pos_offset; - - } -*/ InfoArr[i].xend = RANDFRAC*fabs(2.0*wander_area_pos_offset) + wander_area_pos_offset; InfoArr[i].yend = RANDFRAC*fabs(2.0*wander_area_pos_offset) + wander_area_pos_offset; diff --git a/panda/src/glgsg/glGraphicsStateGuardian.cxx b/panda/src/glgsg/glGraphicsStateGuardian.cxx index cdb0ce8d87..63904ac275 100644 --- a/panda/src/glgsg/glGraphicsStateGuardian.cxx +++ b/panda/src/glgsg/glGraphicsStateGuardian.cxx @@ -215,16 +215,16 @@ reset() { // Set up our clear values to invalid values, so the glClear* calls // will be made initially. - _clear_color_red = -1.0; - _clear_color_green = -1.0; - _clear_color_blue = -1.0; - _clear_color_alpha = -1.0; - _clear_depth = -1.0; + _clear_color_red = -1.0f; + _clear_color_green = -1.0f; + _clear_color_blue = -1.0f; + _clear_color_alpha = -1.0f; + _clear_depth = -1.0f; _clear_stencil = -1; - _clear_accum_red = -1.0; - _clear_accum_green = -1.0; - _clear_accum_blue = -1.0; - _clear_accum_alpha = -1.0; + _clear_accum_red = -1.0f; + _clear_accum_green = -1.0f; + _clear_accum_blue = -1.0f; + _clear_accum_alpha = -1.0f; // Set up the specific state values to GL's known initial values. _draw_buffer_mode = (has_back) ? GL_BACK : GL_FRONT; @@ -232,8 +232,8 @@ reset() { _shade_model_mode = GL_SMOOTH; glFrontFace(GL_CCW); - _line_width = 1.0; - _point_size = 1.0; + _line_width = 1.0f; + _point_size = 1.0f; _depth_mask = false; _fog_mode = GL_EXP; _alpha_func = GL_ALWAYS; @@ -2473,19 +2473,19 @@ issue_transform(const TransformAttribute *attrib) { glBegin(GL_LINES); // X axis in red - glColor3f(1.0, 0.0, 0.0); - glVertex3f(0.0, 0.0, 0.0); - glVertex3f(1.0, 0.0, 0.0); + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(1.0f, 0.0f, 0.0f); // Y axis in green - glColor3f(0.0, 1.0, 0.0); - glVertex3f(0.0, 0.0, 0.0); - glVertex3f(0.0, 1.0, 0.0); + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 1.0f, 0.0f); // Z axis in blue - glColor3f(0.0, 0.0, 1.0); - glVertex3f(0.0, 0.0, 0.0); - glVertex3f(0.0, 0.0, 1.0); + glColor3f(0.0f, 0.0f, 1.0f); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 0.0f, 1.0f); glEnd(); enable_lighting(lighting_was_enabled); @@ -2712,7 +2712,7 @@ void GLGraphicsStateGuardian::issue_light(const LightAttribute *attrib ) // Initialize the current ambient light total and newly enabled // light list - Colorf cur_ambient_light(0.0, 0.0, 0.0, 1.0); + Colorf cur_ambient_light(0.0f, 0.0f, 0.0f, 1.0f); int i; for (i = 0; i < _max_lights; i++) { _light_info[i]._next_enabled = false; @@ -3143,7 +3143,7 @@ issue_polygon_offset(const PolygonOffsetAttribute *attrib) { if(attrib->get_units() != 0 || attrib->get_factor() != 0) { // GLfloat newfactor=attrib->get_factor(); - GLfloat newfactor= 1.0; + GLfloat newfactor= 1.0f; GLfloat newunits=attrib->get_units(); glPolygonOffset(newfactor,newunits); @@ -3233,11 +3233,11 @@ begin_decal(GeomNode *base_geom) { set_state(state, false); #else // use old way instead - glPolygonOffset(0.0,POLYGON_OFFSET_MULTIPLIER * _decal_level); + glPolygonOffset(0.0f,POLYGON_OFFSET_MULTIPLIER * _decal_level); glEnable(GL_POLYGON_OFFSET_FILL); #endif } else { - // GL 1.0-style: use three-step rendering to do decals. + // GL 1.0f-style: use three-step rendering to do decals. if (_decal_level > 1) { // If we're already decaling, just draw the geometry. @@ -3289,14 +3289,14 @@ end_decal(GeomNode *base_geom) { set_state(state, false); #else // use old way instead - glPolygonOffset(0.0,POLYGON_OFFSET_MULTIPLIER * _decal_level); + glPolygonOffset(0.0f,POLYGON_OFFSET_MULTIPLIER * _decal_level); if (_decal_level == 0) { glDisable(GL_POLYGON_OFFSET_FILL); } #endif } else { - // GL 1.0-style: use three-step rendering to do decals. + // GL 1.0f-style: use three-step rendering to do decals. if (_decal_level == 0) { // Now we need to re-render the base geometry with the depth write @@ -4090,16 +4090,16 @@ build_phony_mipmap_level(int level, int xsize, int ysize) { "mipmap_level_9.rgb" }; static const RGBColorf level_colors[num_levels] = { - RGBColorf(1.0, 1.0, 1.0), - RGBColorf(1.0, 0.0, 0.0), - RGBColorf(0.0, 1.0, 0.0), - RGBColorf(0.0, 0.0, 1.0), - RGBColorf(1.0, 1.0, 0.0), - RGBColorf(0.0, 1.0, 1.0), - RGBColorf(1.0, 0.0, 1.0), - RGBColorf(1.0, 0.5, 0.0), - RGBColorf(0.0, 1.0, 0.5), - RGBColorf(0.83, 0.71, 1.0) + RGBColorf(1.0f, 1.0f, 1.0f), + RGBColorf(1.0f, 0.0f, 0.0f), + RGBColorf(0.0f, 1.0f, 0.0f), + RGBColorf(0.0f, 0.0f, 1.0f), + RGBColorf(1.0f, 1.0f, 0.0f), + RGBColorf(0.0f, 1.0f, 1.0f), + RGBColorf(1.0f, 0.0f, 1.0f), + RGBColorf(1.0f, 0.5, 0.0f), + RGBColorf(0.0f, 1.0f, 0.5), + RGBColorf(0.83, 0.71, 1.0f) }; level = level % num_levels; diff --git a/panda/src/linmath/compose_matrix_src.I b/panda/src/linmath/compose_matrix_src.I index ebc0ce1ac0..42c58aad57 100644 --- a/panda/src/linmath/compose_matrix_src.I +++ b/panda/src/linmath/compose_matrix_src.I @@ -53,7 +53,7 @@ decompose_matrix(const FLOATNAME(LMatrix4) &mat, FLOATNAME(LVecBase3) &translate, CoordinateSystem cs) { // Get the translation first. - translate = mat.get_row3(3); + mat.get_row3(translate,3); return decompose_matrix(mat.get_upper_3(), scale, hpr, cs); } @@ -79,7 +79,7 @@ decompose_matrix(const FLOATNAME(LMatrix4) &mat, FLOATTYPE roll, CoordinateSystem cs) { // Get the translation first. - translate = mat.get_row3(3); + mat.get_row3(translate,3); return decompose_matrix(mat.get_upper_3(), scale, hpr, roll, cs); } diff --git a/panda/src/linmath/compose_matrix_src.cxx b/panda/src/linmath/compose_matrix_src.cxx index 49fe3a7f96..6338744fb9 100644 --- a/panda/src/linmath/compose_matrix_src.cxx +++ b/panda/src/linmath/compose_matrix_src.cxx @@ -13,18 +13,20 @@ compose_matrix(FLOATNAME(LMatrix3) &mat, const FLOATNAME(LVecBase3) &scale, const FLOATNAME(LVecBase3) &hpr, CoordinateSystem cs) { + + // temp_hpr_fix blocks use the correct way. need to keep other way as default until + // legacy tools are fixed to work with correct way + if (temp_hpr_fix) { - mat = - FLOATNAME(LMatrix3)::scale_mat(scale) * + mat.scale_multiply(scale, FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::forward(cs), cs) * FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) * - FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs); + FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs)); } else { - mat = - FLOATNAME(LMatrix3)::scale_mat(scale) * + mat.scale_multiply(scale, FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) * FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs) * - FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::back(cs), cs); + FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::back(cs), cs)); } } @@ -39,14 +41,15 @@ compose_matrix(FLOATNAME(LMatrix3) &mat, //////////////////////////////////////////////////////////////////// static void unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { - if (temp_hpr_fix) { - typedef FLOATNAME(LMatrix3) Matrix; + typedef FLOATNAME(LMatrix3) Matrix; + + if (temp_hpr_fix) { // Extract the axes from the matrix. FLOATNAME(LVector3) x, y, z; - x = mat.get_row(0); - y = mat.get_row(1); - z = mat.get_row(2); + mat.get_row(x,0); + mat.get_row(y,1); + mat.get_row(z,2); // Project Z into the XZ plane. FLOATNAME(LVector2) xz(z[0], z[2]); @@ -54,11 +57,11 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { // Compute the rotation about the +Y (up) axis. This is yaw, or // "heading". - FLOATTYPE heading = rad_2_deg(atan2(xz[0], xz[1])); + FLOATTYPE heading = rad_2_deg(((FLOATTYPE)atan2(xz[0], xz[1]))); // Unwind the heading, and continue. Matrix rot_y; - rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0, 1.0, 0.0), + rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f), CS_yup_right); x = x * rot_y; @@ -70,11 +73,11 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { yz = normalize(yz); // Compute the rotation about the +X (right) axis. This is pitch. - FLOATTYPE pitch = rad_2_deg(-atan2(yz[0], yz[1])); + FLOATTYPE pitch = rad_2_deg((FLOATTYPE)(-atan2(yz[0], yz[1]))); // Unwind the pitch. Matrix rot_x; - rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0), + rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f), CS_yup_right); x = x * rot_x; @@ -84,13 +87,13 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { // Project the rotated X onto the XY plane. FLOATNAME(LVector2) xy(x[0], x[1]); xy = normalize(xy); - + // Compute the rotation about the +Z (back) axis. This is roll. - FLOATTYPE roll = -rad_2_deg(atan2(xy[1], xy[0])); + FLOATTYPE roll = -rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0]))); // Unwind the roll from the axes, and continue. Matrix rot_z; - rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0, 0.0, 1.0), + rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f), CS_yup_right); x = x * rot_z; @@ -107,24 +110,23 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { hpr[1] = pitch; hpr[2] = roll; } else { - typedef FLOATNAME(LMatrix3) Matrix; // Extract the axes from the matrix. FLOATNAME(LVector3) x, y, z; - x = mat.get_row(0); - y = mat.get_row(1); - z = mat.get_row(2); + mat.get_row(x,0); + mat.get_row(y,1); + mat.get_row(z,2); // Project X onto the XY plane. FLOATNAME(LVector2) xy(x[0], x[1]); xy = normalize(xy); // Compute the rotation about the +Z (back) axis. This is roll. - FLOATTYPE roll = rad_2_deg(atan2(xy[1], xy[0])); + FLOATTYPE roll = rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0]))); // Unwind the roll from the axes, and continue. Matrix rot_z; - rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0, 0.0, 1.0), + rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f), CS_yup_right); x = x * rot_z; @@ -137,11 +139,11 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { // Compute the rotation about the +Y (up) axis. This is yaw, or // "heading". - FLOATTYPE heading = rad_2_deg(-atan2(xz[1], xz[0])); + FLOATTYPE heading = rad_2_deg(((FLOATTYPE)-atan2(xz[1], xz[0]))); // Unwind the heading, and continue. Matrix rot_y; - rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0, 1.0, 0.0), + rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f), CS_yup_right); x = x * rot_y; @@ -151,13 +153,13 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { // Project the rotated Z into the YZ plane. FLOATNAME(LVector2) yz(z[1], z[2]); yz = normalize(yz); - + // Compute the rotation about the +X (right) axis. This is pitch. - FLOATTYPE pitch = rad_2_deg(-atan2(yz[0], yz[1])); + FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)-atan2(yz[0], yz[1]))); // Unwind the pitch. Matrix rot_x; - rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0), + rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f), CS_yup_right); x = x * rot_x; @@ -198,13 +200,13 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr, // Extract the axes from the matrix. FLOATNAME(LVector3) x, y, z; - x = mat.get_row(0); - y = mat.get_row(1); - z = mat.get_row(2); + mat.get_row(x,0); + mat.get_row(y,1); + mat.get_row(z,2); // Unwind the roll from the axes, and continue. Matrix rot_z; - rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0, 0.0, 1.0), + rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f), CS_yup_right); x = x * rot_z; @@ -217,11 +219,11 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr, // Compute the rotation about the +Y (up) axis. This is yaw, or // "heading". - FLOATTYPE heading = rad_2_deg(-atan2(xz[1], xz[0])); + FLOATTYPE heading = rad_2_deg(((FLOATTYPE)-atan2(xz[1], xz[0]))); // Unwind the heading, and continue. Matrix rot_y; - rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0, 1.0, 0.0), + rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f), CS_yup_right); x = x * rot_y; @@ -233,11 +235,11 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr, yz = normalize(yz); // Compute the rotation about the +X (right) axis. This is pitch. - FLOATTYPE pitch = rad_2_deg(-atan2(yz[0], yz[1])); + FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)-atan2(yz[0], yz[1]))); // Unwind the pitch. Matrix rot_x; - rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0), + rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f), CS_yup_right); x = x * rot_x; @@ -271,9 +273,9 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { // Extract the axes from the matrix. FLOATNAME(LVector3) x, y, z; - x = mat.get_row(0); - y = mat.get_row(1); - z = mat.get_row(2); + mat.get_row(x,0); + mat.get_row(y,1); + mat.get_row(z,2); // Project Y into the XY plane. FLOATNAME(LVector2) xy(y[0], y[1]); @@ -281,11 +283,11 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { // Compute the rotation about the +Z (up) axis. This is yaw, or // "heading". - FLOATTYPE heading = -rad_2_deg(atan2(xy[0], xy[1])); + FLOATTYPE heading = -rad_2_deg(((FLOATTYPE)atan2(xy[0], xy[1]))); // Unwind the heading, and continue. Matrix rot_z; - rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0, 0.0, 1.0), + rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f), CS_zup_right); x = x * rot_z; @@ -297,11 +299,11 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { yz = normalize(yz); // Compute the rotation about the +X (right) axis. This is pitch. - FLOATTYPE pitch = rad_2_deg(atan2(yz[1], yz[0])); + FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)atan2(yz[1], yz[0]))); // Unwind the pitch. Matrix rot_x; - rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0), + rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f), CS_zup_right); x = x * rot_x; @@ -313,11 +315,11 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { xz = normalize(xz); // Compute the rotation about the -Y (back) axis. This is roll. - FLOATTYPE roll = -rad_2_deg(atan2(xz[1], xz[0])); + FLOATTYPE roll = -rad_2_deg(((FLOATTYPE)atan2(xz[1], xz[0]))); // Unwind the roll from the axes, and continue. Matrix rot_y; - rot_y = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0, 1.0, 0.0), + rot_y = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f), CS_zup_right); x = x * rot_y; @@ -339,9 +341,9 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { // Extract the axes from the matrix. FLOATNAME(LVector3) x, y, z; - x = mat.get_row(0); - y = mat.get_row(1); - z = mat.get_row(2); + mat.get_row(x,0); + mat.get_row(y,1); + mat.get_row(z,2); // Project X into the XZ plane. @@ -349,10 +351,10 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { xz = normalize(xz); // Compute the rotation about the -Y (back) axis. This is roll. - FLOATTYPE roll = rad_2_deg(atan2(xz[1], xz[0])); + FLOATTYPE roll = rad_2_deg(((FLOATTYPE)atan2(xz[1], xz[0]))); - if (y[1] < 0.0) { - if (roll < 0.0) { + if (y[1] < 0.0f) { + if (roll < 0.0f) { roll += 180.0; } else { roll -= 180.0; @@ -361,7 +363,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { // Unwind the roll from the axes, and continue. Matrix rot_y; - rot_y = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0, 1.0, 0.0), + rot_y = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f), CS_zup_right); x = x * rot_y; @@ -374,11 +376,11 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { // Compute the rotation about the +Z (up) axis. This is yaw, or // "heading". - FLOATTYPE heading = rad_2_deg(atan2(xy[1], xy[0])); + FLOATTYPE heading = rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0]))); // Unwind the heading, and continue. Matrix rot_z; - rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0, 0.0, 1.0), + rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f), CS_zup_right); x = x * rot_z; @@ -390,11 +392,11 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { yz = normalize(yz); // Compute the rotation about the +X (right) axis. This is pitch. - FLOATTYPE pitch = rad_2_deg(atan2(yz[1], yz[0])); + FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)atan2(yz[1], yz[0]))); // Unwind the pitch. Matrix rot_x; - rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0), + rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f), CS_zup_right); x = x * rot_x; @@ -435,13 +437,13 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr, // Extract the axes from the matrix. FLOATNAME(LVector3) x, y, z; - x = mat.get_row(0); - y = mat.get_row(1); - z = mat.get_row(2); + mat.get_row(x,0); + mat.get_row(y,1); + mat.get_row(z,2); // Unwind the roll from the axes, and continue. Matrix rot_y; - rot_y = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0, 1.0, 0.0), + rot_y = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f), CS_zup_right); x = x * rot_y; @@ -454,11 +456,11 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr, // Compute the rotation about the +Z (up) axis. This is yaw, or // "heading". - FLOATTYPE heading = rad_2_deg(atan2(xy[1], xy[0])); + FLOATTYPE heading = rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0]))); // Unwind the heading, and continue. Matrix rot_z; - rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0, 0.0, 1.0), + rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f), CS_zup_right); x = x * rot_z; @@ -470,11 +472,11 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr, yz = normalize(yz); // Compute the rotation about the +X (right) axis. This is pitch. - FLOATTYPE pitch = rad_2_deg(atan2(yz[1], yz[0])); + FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)atan2(yz[1], yz[0]))); // Unwind the pitch. Matrix rot_x; - rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0), + rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f), CS_zup_right); x = x * rot_x; @@ -510,66 +512,54 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat, // Extract the rotation and scale, according to the coordinate // system of choice. - bool shear; + bool bMatHasNoShear,bIsLeftHandedMat; + + FLOATNAME(LMatrix3) new_mat(mat); switch (cs) { case CS_zup_right: { - FLOATNAME(LMatrix3) rm(mat); - unwind_zup_rotation(rm, hpr); - scale[0] = rm(0, 0); - scale[1] = rm(1, 1); - scale[2] = rm(2, 2); - shear = - (fabs(rm(0, 1)) + fabs(rm(0, 2)) + - fabs(rm(1, 0)) + fabs(rm(1, 2)) + - fabs(rm(2, 0)) + fabs(rm(2, 1))) >= 0.0001; + unwind_zup_rotation(new_mat, hpr); + bIsLeftHandedMat = false; } break; case CS_yup_right: { - FLOATNAME(LMatrix3) rm(mat); - unwind_yup_rotation(rm, hpr); - scale[0] = rm(0, 0); - scale[1] = rm(1, 1); - scale[2] = rm(2, 2); - shear = - (fabs(rm(0, 1)) + fabs(rm(0, 2)) + - fabs(rm(1, 0)) + fabs(rm(1, 2)) + - fabs(rm(2, 0)) + fabs(rm(2, 1))) >= 0.0001; - } + unwind_yup_rotation(new_mat, hpr); + bIsLeftHandedMat = false; + } break; case CS_zup_left: { + new_mat._m.m._02 = -new_mat._m.m._02; + new_mat._m.m._12 = -new_mat._m.m._12; + new_mat._m.m._20 = -new_mat._m.m._20; + new_mat._m.m._21 = -new_mat._m.m._21; +/* FLOATNAME(LMatrix3) lm(mat(0, 0), mat(0, 1), -mat(0, 2), mat(1, 0), mat(1, 1), -mat(1, 2), -mat(2, 0), -mat(2, 1), mat(2, 2)); - unwind_zup_rotation(lm, hpr); - scale[0] = -lm(0, 0); - scale[1] = -lm(1, 1); - scale[2] = lm(2, 2); - shear = - (fabs(lm(0, 1)) + fabs(lm(0, 2)) + - fabs(lm(1, 0)) + fabs(lm(1, 2)) + - fabs(lm(2, 0)) + fabs(lm(2, 1))) >= 0.0001; +*/ + unwind_zup_rotation(new_mat, hpr); + bIsLeftHandedMat = true; } break; case CS_yup_left: { + new_mat._m.m._02 = -new_mat._m.m._02; + new_mat._m.m._12 = -new_mat._m.m._12; + new_mat._m.m._20 = -new_mat._m.m._20; + new_mat._m.m._21 = -new_mat._m.m._21; +/* FLOATNAME(LMatrix3) lm(mat(0, 0), mat(0, 1), -mat(0, 2), mat(1, 0), mat(1, 1), -mat(1, 2), -mat(2, 0), -mat(2, 1), mat(2, 2)); - unwind_yup_rotation(lm, hpr); - scale[0] = -lm(0, 0); - scale[1] = -lm(1, 1); - scale[2] = lm(2, 2); - shear = - (fabs(lm(0, 1)) + fabs(lm(0, 2)) + - fabs(lm(1, 0)) + fabs(lm(1, 2)) + - fabs(lm(2, 0)) + fabs(lm(2, 1))) >= 0.0001; +*/ + unwind_yup_rotation(new_mat, hpr); + bIsLeftHandedMat = true; } break; @@ -579,7 +569,21 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat, return false; } - return !shear; + scale[2] = new_mat._m.m._22; + if(bIsLeftHandedMat) { + scale[0] = -new_mat._m.m._00; + scale[1] = -new_mat._m.m._11; + } else { + scale[0] = new_mat._m.m._00; + scale[1] = new_mat._m.m._11; + } + + bMatHasNoShear = + (fabs(new_mat(0, 1)) + fabs(new_mat(0, 2)) + + fabs(new_mat(1, 0)) + fabs(new_mat(1, 2)) + + fabs(new_mat(2, 0)) + fabs(new_mat(2, 1))) < 0.0001; + + return bMatHasNoShear; } //////////////////////////////////////////////////////////////////// @@ -607,66 +611,54 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat, // Extract the rotation and scale, according to the coordinate // system of choice. - bool shear; + bool bMatHasNoShear,bIsLeftHandedMat; + + FLOATNAME(LMatrix3) new_mat(mat); switch (cs) { case CS_zup_right: { - FLOATNAME(LMatrix3) rm(mat); - unwind_zup_rotation(rm, hpr, roll); - scale[0] = rm(0, 0); - scale[1] = rm(1, 1); - scale[2] = rm(2, 2); - shear = - (fabs(rm(0, 1)) + fabs(rm(0, 2)) + - fabs(rm(1, 0)) + fabs(rm(1, 2)) + - fabs(rm(2, 0)) + fabs(rm(2, 1))) >= 0.0001; + unwind_zup_rotation(new_mat, hpr, roll); + bIsLeftHandedMat = false; } break; case CS_yup_right: { - FLOATNAME(LMatrix3) rm(mat); - unwind_yup_rotation(rm, hpr, roll); - scale[0] = rm(0, 0); - scale[1] = rm(1, 1); - scale[2] = rm(2, 2); - shear = - (fabs(rm(0, 1)) + fabs(rm(0, 2)) + - fabs(rm(1, 0)) + fabs(rm(1, 2)) + - fabs(rm(2, 0)) + fabs(rm(2, 1))) >= 0.0001; - } + unwind_yup_rotation(new_mat, hpr, roll); + bIsLeftHandedMat = false; + } break; case CS_zup_left: { + new_mat._m.m._02 = -new_mat._m.m._02; + new_mat._m.m._12 = -new_mat._m.m._12; + new_mat._m.m._20 = -new_mat._m.m._20; + new_mat._m.m._21 = -new_mat._m.m._21; +/* FLOATNAME(LMatrix3) lm(mat(0, 0), mat(0, 1), -mat(0, 2), mat(1, 0), mat(1, 1), -mat(1, 2), -mat(2, 0), -mat(2, 1), mat(2, 2)); - unwind_zup_rotation(lm, hpr, roll); - scale[0] = -lm(0, 0); - scale[1] = -lm(1, 1); - scale[2] = lm(2, 2); - shear = - (fabs(lm(0, 1)) + fabs(lm(0, 2)) + - fabs(lm(1, 0)) + fabs(lm(1, 2)) + - fabs(lm(2, 0)) + fabs(lm(2, 1))) >= 0.0001; +*/ + unwind_zup_rotation(new_mat, hpr, roll); + bIsLeftHandedMat = true; } break; case CS_yup_left: { + new_mat._m.m._02 = -new_mat._m.m._02; + new_mat._m.m._12 = -new_mat._m.m._12; + new_mat._m.m._20 = -new_mat._m.m._20; + new_mat._m.m._21 = -new_mat._m.m._21; +/* FLOATNAME(LMatrix3) lm(mat(0, 0), mat(0, 1), -mat(0, 2), mat(1, 0), mat(1, 1), -mat(1, 2), -mat(2, 0), -mat(2, 1), mat(2, 2)); - unwind_yup_rotation(lm, hpr, roll); - scale[0] = -lm(0, 0); - scale[1] = -lm(1, 1); - scale[2] = lm(2, 2); - shear = - (fabs(lm(0, 1)) + fabs(lm(0, 2)) + - fabs(lm(1, 0)) + fabs(lm(1, 2)) + - fabs(lm(2, 0)) + fabs(lm(2, 1))) >= 0.0001; +*/ + unwind_yup_rotation(new_mat, hpr, roll); + bIsLeftHandedMat = true; } break; @@ -676,5 +668,20 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat, return false; } - return !shear; + scale[2] = new_mat._m.m._22; + if(bIsLeftHandedMat) { + scale[0] = -new_mat._m.m._00; + scale[1] = -new_mat._m.m._11; + } else { + scale[0] = new_mat._m.m._00; + scale[1] = new_mat._m.m._11; + } + + bMatHasNoShear = + (fabs(new_mat(0, 1)) + fabs(new_mat(0, 2)) + + fabs(new_mat(1, 0)) + fabs(new_mat(1, 2)) + + fabs(new_mat(2, 0)) + fabs(new_mat(2, 1))) < 0.0001; + + return bMatHasNoShear; } + diff --git a/panda/src/linmath/dblnames.h b/panda/src/linmath/dblnames.h index 404285b4e5..a7a98b3102 100644 --- a/panda/src/linmath/dblnames.h +++ b/panda/src/linmath/dblnames.h @@ -27,7 +27,10 @@ #undef FLOATTYPE #undef FLOATNAME #undef FLOATTOKEN +#undef FLOATCONST #define FLOATTYPE double #define FLOATNAME(ARG) ARG##d #define FLOATTOKEN 'd' +#define FLOATCONST(ARG) ARG + diff --git a/panda/src/linmath/deg_2_rad.h b/panda/src/linmath/deg_2_rad.h index 8d2f68912d..aa58d1f75f 100644 --- a/panda/src/linmath/deg_2_rad.h +++ b/panda/src/linmath/deg_2_rad.h @@ -11,8 +11,11 @@ #include "mathNumbers.h" BEGIN_PUBLISH -INLINE_LINMATH double deg_2_rad( double f ) { return f * MathNumbers::pi / 180.0; } -INLINE_LINMATH double rad_2_deg( double f ) { return f * 180.0 / MathNumbers::pi; } +INLINE_LINMATH double deg_2_rad( double f ) { return f * (MathNumbers::pi / 180.0); } +INLINE_LINMATH double rad_2_deg( double f ) { return f * (180.0 / MathNumbers::pi); } + +INLINE_LINMATH float deg_2_rad( float f ) { return f * (MathNumbers::pi_f / 180.0f); } +INLINE_LINMATH float rad_2_deg( float f ) { return f * (180.0f / MathNumbers::pi_f); } END_PUBLISH #endif diff --git a/panda/src/linmath/fltnames.h b/panda/src/linmath/fltnames.h index 1f043c6586..9c84793359 100644 --- a/panda/src/linmath/fltnames.h +++ b/panda/src/linmath/fltnames.h @@ -27,7 +27,9 @@ #undef FLOATTYPE #undef FLOATNAME #undef FLOATTOKEN +#undef FLOATCONST #define FLOATTYPE float #define FLOATNAME(ARG) ARG##f #define FLOATTOKEN 'f' +#define FLOATCONST(ARG) ARG##f diff --git a/panda/src/linmath/lmatrix3_src.I b/panda/src/linmath/lmatrix3_src.I index eafd98d7d6..3c6f5096ec 100644 --- a/panda/src/linmath/lmatrix3_src.I +++ b/panda/src/linmath/lmatrix3_src.I @@ -177,6 +177,13 @@ get_row(int row) const { return FLOATNAME(LVecBase3)((*this)(row, 0), (*this)(row, 1), (*this)(row, 2)); } +INLINE_LINMATH void FLOATNAME(LMatrix3):: +get_row(FLOATNAME(LVecBase3) &result_vec,int row) const { + result_vec._v.v._0 = (*this)(row, 0); + result_vec._v.v._1 = (*this)(row, 1); + result_vec._v.v._2 = (*this)(row, 2); +} + //////////////////////////////////////////////////////////////////// // Function: LMatrix3::get_col // Access: Public @@ -229,8 +236,8 @@ operator () (int row, int col) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATTYPE FLOATNAME(LMatrix3):: operator () (int row, int col) const { -// nassertr(row >= 0 && row < 3, 0.0); -// nassertr(col >= 0 && col < 3, 0.0); +// nassertr(row >= 0 && row < 3, 0.0f); +// nassertr(col >= 0 && col < 3, 0.0f); return _m.data[row * 3 + col]; } @@ -255,8 +262,8 @@ is_nan() const { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATTYPE FLOATNAME(LMatrix3):: get_cell(int row, int col) const { -// nassertr(row >= 0 && row < 3, 0.0); -// nassertr(col >= 0 && col < 3, 0.0); +// nassertr(row >= 0 && row < 3, 0.0f); +// nassertr(col >= 0 && col < 3, 0.0f); return _m.data[row * 3 + col]; } @@ -426,7 +433,7 @@ xform_point(const FLOATNAME(LVecBase2) &v) const { FLOATNAME(LVecBase2) v_res; - // v._v.v._2 == 1.0 for this case + // v._v.v._2 == 1.0f for this case v_res._v.v._0 = v._v.v._0*_m.m._00 + v._v.v._1*_m.m._10 + _m.m._20; v_res._v.v._1 = v._v.v._0*_m.m._01 + v._v.v._1*_m.m._11 + _m.m._21; @@ -449,7 +456,7 @@ xform_vec(const FLOATNAME(LVecBase2) &v) const { FLOATNAME(LVecBase2) v_res; - // v._v.v._2 == 0.0 for this case + // v._v.v._2 == 0.0f for this case v_res._v.v._0 = v._v.v._0*_m.m._00 + v._v.v._1*_m.m._10; v_res._v.v._1 = v._v.v._0*_m.m._01 + v._v.v._1*_m.m._11; @@ -529,6 +536,41 @@ typedef union { return t; } +// this = other1 * other2 +INLINE_LINMATH void FLOATNAME(LMatrix3):: +multiply(const FLOATNAME(LMatrix3) &other1, const FLOATNAME(LMatrix3) &other2) { +// faster than operator * since it writes result in place, avoiding extra copying +// this will fail if you try to mat.multiply(mat,other_mat) + + #ifdef _DEBUG + assert((&other1 != this) && (&other2 != this)); + #endif + + MATRIX3_PRODUCT((*this),other1,other2); +} + +// this = scale_mat(scale_vector) * other_mat, efficiently +INLINE_LINMATH void FLOATNAME(LMatrix3):: +scale_multiply(const FLOATNAME(LVecBase3) &scale_vector,const FLOATNAME(LMatrix3) &other_mat) { + #ifdef _DEBUG + assert(&other_mat != this); + #endif + + // optimize for 0.0 or 1.0 factors? + + _m.m._00 = other_mat._m.m._00 * scale_vector._v.v._0; + _m.m._01 = other_mat._m.m._01 * scale_vector._v.v._0; + _m.m._02 = other_mat._m.m._02 * scale_vector._v.v._0; + + _m.m._10 = other_mat._m.m._10 * scale_vector._v.v._1; + _m.m._11 = other_mat._m.m._11 * scale_vector._v.v._1; + _m.m._12 = other_mat._m.m._12 * scale_vector._v.v._1; + + _m.m._20 = other_mat._m.m._20 * scale_vector._v.v._2; + _m.m._21 = other_mat._m.m._21 * scale_vector._v.v._2; + _m.m._22 = other_mat._m.m._22 * scale_vector._v.v._2; +} + //////////////////////////////////////////////////////////////////// // Function: LMatrix3::matrix * scalar // Access: Public @@ -560,7 +602,7 @@ operator * (FLOATTYPE scalar) const { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LMatrix3) FLOATNAME(LMatrix3):: operator / (FLOATTYPE scalar) const { - FLOATTYPE recip_scalar = 1.0/scalar; + FLOATTYPE recip_scalar = 1.0f/scalar; return (*this) * recip_scalar; } @@ -650,7 +692,7 @@ operator *= (FLOATTYPE scalar) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LMatrix3) &FLOATNAME(LMatrix3):: operator /= (FLOATTYPE scalar) { - FLOATTYPE recip_scalar = 1.0/scalar; + FLOATTYPE recip_scalar = 1.0f/scalar; _m.m._00 *= recip_scalar; _m.m._01 *= recip_scalar; _m.m._02 *= recip_scalar; @@ -771,7 +813,7 @@ invert_from(const FLOATNAME(LMatrix3) &other) { return false; } - other_det = 1.0 / other_det; + other_det = 1.0f / other_det; _m.m._00 = other_det * DET2(other._m.m._11, other._m.m._12, other._m.m._21, other._m.m._22); _m.m._10 = -other_det * DET2(other._m.m._10, other._m.m._12, other._m.m._20, other._m.m._22); _m.m._20 = other_det * DET2(other._m.m._10, other._m.m._11, other._m.m._20, other._m.m._21); @@ -808,9 +850,9 @@ invert_in_place() { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LMatrix3) FLOATNAME(LMatrix3):: translate_mat(const FLOATNAME(LVecBase2) &trans) { - return FLOATNAME(LMatrix3)(1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - trans._v.v._0, trans._v.v._1, 1.0); + return FLOATNAME(LMatrix3)(1.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 0.0f, + trans._v.v._0, trans._v.v._1, 1.0f); } //////////////////////////////////////////////////////////////////// @@ -821,9 +863,9 @@ translate_mat(const FLOATNAME(LVecBase2) &trans) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LMatrix3) FLOATNAME(LMatrix3):: translate_mat(FLOATTYPE tx, FLOATTYPE ty) { - return FLOATNAME(LMatrix3)(1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - tx, ty, 1.0); + return FLOATNAME(LMatrix3)(1.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 0.0f, + tx, ty, 1.0f); } //////////////////////////////////////////////////////////////////// @@ -837,9 +879,9 @@ rotate_mat(FLOATTYPE angle) { FLOATTYPE angle_rad=deg_2_rad(angle); FLOATTYPE s,c; csincos(angle_rad,&s,&c); - return FLOATNAME(LMatrix3)( c, s, 0.0, - -s, c, 0.0, - 0.0, 0.0, 1.0); + return FLOATNAME(LMatrix3)( c, s, 0.0f, + -s, c, 0.0f, + 0.0f, 0.0f, 1.0f); } //////////////////////////////////////////////////////////////////// @@ -850,9 +892,9 @@ rotate_mat(FLOATTYPE angle) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LMatrix3) FLOATNAME(LMatrix3):: scale_mat(const FLOATNAME(LVecBase2) &scale) { - return FLOATNAME(LMatrix3)(scale._v.v._0, 0.0, 0.0, - 0.0, scale._v.v._1, 0.0, - 0.0, 0.0, 1.0); + return FLOATNAME(LMatrix3)(scale._v.v._0, 0.0f, 0.0f, + 0.0f, scale._v.v._1, 0.0f, + 0.0f, 0.0f, 1.0f); } //////////////////////////////////////////////////////////////////// @@ -863,9 +905,9 @@ scale_mat(const FLOATNAME(LVecBase2) &scale) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LMatrix3) FLOATNAME(LMatrix3):: scale_mat(FLOATTYPE sx, FLOATTYPE sy) { - return FLOATNAME(LMatrix3)(sx, 0.0, 0.0, - 0.0, sy, 0.0, - 0.0, 0.0, 1.0); + return FLOATNAME(LMatrix3)(sx, 0.0f, 0.0f, + 0.0f, sy, 0.0f, + 0.0f, 0.0f, 1.0f); } //////////////////////////////////////////////////////////////////// @@ -895,9 +937,9 @@ rotate_mat(FLOATTYPE angle, FLOATNAME(LVecBase3) axis, // Normalize the axis. FLOATTYPE length_sq = axis_0 * axis_0 + axis_1 * axis_1 + axis_2 * axis_2; #ifdef _DEBUG - nassertr(length_sq != 0.0, ident_mat()); + nassertr(length_sq != 0.0f, ident_mat()); #endif - FLOATTYPE recip_length = 1.0/csqrt(length_sq); + FLOATTYPE recip_length = 1.0f/csqrt(length_sq); axis_0 *= recip_length; axis_1 *= recip_length; @@ -906,7 +948,7 @@ rotate_mat(FLOATTYPE angle, FLOATNAME(LVecBase3) axis, FLOATTYPE angle_rad=deg_2_rad(angle); FLOATTYPE s,c; csincos(angle_rad,&s,&c); - FLOATTYPE t = 1.0 - c; + FLOATTYPE t = 1.0f - c; FLOATTYPE t0,t1,t2,s0,s1,s2; @@ -974,7 +1016,7 @@ rotate_mat_normaxis(FLOATTYPE angle, const FLOATNAME(LVecBase3) &axis, FLOATTYPE angle_rad=deg_2_rad(angle); FLOATTYPE s,c; csincos(angle_rad,&s,&c); - FLOATTYPE t = 1.0 - c; + FLOATTYPE t = 1.0f - c; FLOATTYPE t0,t1,t2,s0,s1,s2; @@ -1022,9 +1064,9 @@ rotate_mat_normaxis(FLOATTYPE angle, const FLOATNAME(LVecBase3) &axis, //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LMatrix3) FLOATNAME(LMatrix3):: scale_mat(const FLOATNAME(LVecBase3) &scale) { - return FLOATNAME(LMatrix3)(scale[0], 0.0, 0.0, - 0.0, scale._v.v._1, 0.0, - 0.0, 0.0, scale._v.v._2); + return FLOATNAME(LMatrix3)(scale[0], 0.0f, 0.0f, + 0.0f, scale._v.v._1, 0.0f, + 0.0f, 0.0f, scale._v.v._2); } //////////////////////////////////////////////////////////////////// @@ -1035,9 +1077,9 @@ scale_mat(const FLOATNAME(LVecBase3) &scale) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LMatrix3) FLOATNAME(LMatrix3):: scale_mat(FLOATTYPE sx, FLOATTYPE sy, FLOATTYPE sz) { - return FLOATNAME(LMatrix3)(sx, 0.0, 0.0, - 0.0, sy, 0.0, - 0.0, 0.0, sz); + return FLOATNAME(LMatrix3)(sx, 0.0f, 0.0f, + 0.0f, sy, 0.0f, + 0.0f, 0.0f, sz); } //////////////////////////////////////////////////////////////////// diff --git a/panda/src/linmath/lmatrix3_src.cxx b/panda/src/linmath/lmatrix3_src.cxx index 2553e18b7b..f4035ec53b 100644 --- a/panda/src/linmath/lmatrix3_src.cxx +++ b/panda/src/linmath/lmatrix3_src.cxx @@ -6,9 +6,9 @@ TypeHandle FLOATNAME(LMatrix3)::_type_handle; FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_ident_mat = - FLOATNAME(LMatrix3)(1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0); + FLOATNAME(LMatrix3)(1.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 0.0f, + 0.0f, 0.0f, 1.0f); //////////////////////////////////////////////////////////////////// // Function: LMatrix3::fill diff --git a/panda/src/linmath/lmatrix3_src.h b/panda/src/linmath/lmatrix3_src.h index b7789cf07e..165758cf48 100644 --- a/panda/src/linmath/lmatrix3_src.h +++ b/panda/src/linmath/lmatrix3_src.h @@ -41,6 +41,9 @@ PUBLISHED: INLINE_LINMATH FLOATNAME(LVecBase2) get_row2(int row) const; INLINE_LINMATH FLOATNAME(LVecBase2) get_col2(int col) const; + // these versions inline better + INLINE_LINMATH void get_row(FLOATNAME(LVecBase3) &result_vec, int row) const; + INLINE_LINMATH FLOATTYPE &operator () (int row, int col); INLINE_LINMATH FLOATTYPE operator () (int row, int col) const; @@ -76,10 +79,16 @@ PUBLISHED: INLINE_LINMATH FLOATNAME(LVecBase2) xform_vec(const FLOATNAME(LVecBase2) &v) const; + // this = other1 * other2 + INLINE_LINMATH void multiply(const FLOATNAME(LMatrix3) &other1, const FLOATNAME(LMatrix3) &other2); + INLINE_LINMATH FLOATNAME(LMatrix3) operator * (const FLOATNAME(LMatrix3) &other) const; INLINE_LINMATH FLOATNAME(LMatrix3) operator * (FLOATTYPE scalar) const; INLINE_LINMATH FLOATNAME(LMatrix3) operator / (FLOATTYPE scalar) const; + // this = scale_mat(scale_vector) * other_mat, efficiently + INLINE_LINMATH void scale_multiply(const FLOATNAME(LVecBase3) &scale_vector,const FLOATNAME(LMatrix3) &other_mat); + INLINE_LINMATH FLOATNAME(LMatrix3) &operator += (const FLOATNAME(LMatrix3) &other); INLINE_LINMATH FLOATNAME(LMatrix3) &operator -= (const FLOATNAME(LMatrix3) &other); diff --git a/panda/src/linmath/lmatrix4_src.I b/panda/src/linmath/lmatrix4_src.I index 2d86a63096..beff4a01bc 100644 --- a/panda/src/linmath/lmatrix4_src.I +++ b/panda/src/linmath/lmatrix4_src.I @@ -107,27 +107,27 @@ FLOATNAME(LMatrix4)(const FLOATNAME(LMatrix3) &upper3) { _m.m._00 = upper3._m.m._00; _m.m._01 = upper3._m.m._01; _m.m._02 = upper3._m.m._02; - _m.m._03 = 0.0; + _m.m._03 = 0.0f; _m.m._10 = upper3._m.m._10; _m.m._11 = upper3._m.m._11; _m.m._12 = upper3._m.m._12; - _m.m._13 = 0.0; + _m.m._13 = 0.0f; _m.m._20 = upper3._m.m._20; _m.m._21 = upper3._m.m._21; _m.m._22 = upper3._m.m._22; - _m.m._23 = 0.0; + _m.m._23 = 0.0f; - _m.m._30 = 0.0; - _m.m._31 = 0.0; - _m.m._32 = 0.0; - _m.m._33 = 1.0; + _m.m._30 = 0.0f; + _m.m._31 = 0.0f; + _m.m._32 = 0.0f; + _m.m._33 = 1.0f; -// set(upper3(0, 0), upper3(0, 1), upper3(0, 2), 0.0, -// upper3(1, 0), upper3(1, 1), upper3(1, 2), 0.0, -// upper3(2, 0), upper3(2, 1), upper3(2, 2), 0.0, -// 0.0, 0.0, 0.0, 1.0); +// set(upper3(0, 0), upper3(0, 1), upper3(0, 2), 0.0f, +// upper3(1, 0), upper3(1, 1), upper3(1, 2), 0.0f, +// upper3(2, 0), upper3(2, 1), upper3(2, 2), 0.0f, +// 0.0f, 0.0f, 0.0f, 1.0f); } //////////////////////////////////////////////////////////////////// @@ -142,27 +142,27 @@ FLOATNAME(LMatrix4)(const FLOATNAME(LMatrix3) &upper3, _m.m._00 = upper3._m.m._00; _m.m._01 = upper3._m.m._01; _m.m._02 = upper3._m.m._02; - _m.m._03 = 0.0; + _m.m._03 = 0.0f; _m.m._10 = upper3._m.m._10; _m.m._11 = upper3._m.m._11; _m.m._12 = upper3._m.m._12; - _m.m._13 = 0.0; + _m.m._13 = 0.0f; _m.m._20 = upper3._m.m._20; _m.m._21 = upper3._m.m._21; _m.m._22 = upper3._m.m._22; - _m.m._23 = 0.0; + _m.m._23 = 0.0f; _m.m._30 = trans._v.v._0; _m.m._31 = trans._v.v._1; _m.m._32 = trans._v.v._2; - _m.m._33 = 1.0; + _m.m._33 = 1.0f; -// set(upper3(0, 0), upper3(0, 1), upper3(0, 2), 0.0, -// upper3(1, 0), upper3(1, 1), upper3(1, 2), 0.0, -// upper3(2, 0), upper3(2, 1), upper3(2, 2), 0.0, -// trans[0], trans[1], trans[2], 1.0); +// set(upper3(0, 0), upper3(0, 1), upper3(0, 2), 0.0f, +// upper3(1, 0), upper3(1, 1), upper3(1, 2), 0.0f, +// upper3(2, 0), upper3(2, 1), upper3(2, 2), 0.0f, +// trans[0], trans[1], trans[2], 1.0f); } //////////////////////////////////////////////////////////////////// @@ -340,6 +340,13 @@ get_row3(int row) const { (*this)(row, 2)); } +INLINE_LINMATH void FLOATNAME(LMatrix4):: +get_row3(FLOATNAME(LVecBase3) &result_vec,int row) const { + result_vec._v.v._0 = (*this)(row, 0); + result_vec._v.v._1 = (*this)(row, 1); + result_vec._v.v._2 = (*this)(row, 2); +} + //////////////////////////////////////////////////////////////////// // Function: LMatrix4::get_col3 // Access: Public @@ -372,8 +379,8 @@ operator () (int row, int col) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATTYPE FLOATNAME(LMatrix4):: operator () (int row, int col) const { - // nassertr(row >= 0 && row < 4, 0.0); - // nassertr(col >= 0 && col < 4, 0.0); + // nassertr(row >= 0 && row < 4, 0.0f); + // nassertr(col >= 0 && col < 4, 0.0f); return _m.data[row * 4 + col]; } @@ -399,8 +406,8 @@ is_nan() const { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATTYPE FLOATNAME(LMatrix4):: get_cell(int row, int col) const { -// nassertr(row >= 0 && row < 4, 0.0); -// nassertr(col >= 0 && col < 4, 0.0); +// nassertr(row >= 0 && row < 4, 0.0f); +// nassertr(col >= 0 && col < 4, 0.0f); return _m.data[row * 4 + col]; } @@ -565,7 +572,7 @@ xform_point(const FLOATNAME(LVecBase3) &v) const { FLOATNAME(LVecBase3) v_res; - // v._v.v._3 == 1.0 for this case + // v._v.v._3 == 1.0f for this case v_res._v.v._0 = v._v.v._0*_m.m._00 + v._v.v._1*_m.m._10 + v._v.v._2*_m.m._20 + _m.m._30; v_res._v.v._1 = v._v.v._0*_m.m._01 + v._v.v._1*_m.m._11 + v._v.v._2*_m.m._21 + _m.m._31; @@ -591,7 +598,7 @@ INLINE_LINMATH FLOATNAME(LVecBase3) FLOATNAME(LMatrix4):: xform_vec(const FLOATNAME(LVecBase3) &v) const { FLOATNAME(LVecBase3) v_res; - // v._v.v._3 == 0.0 for this case + // v._v.v._3 == 0.0f for this case v_res._v.v._0 = v._v.v._0*_m.m._00 + v._v.v._1*_m.m._10 + v._v.v._2*_m.m._20; v_res._v.v._1 = v._v.v._0*_m.m._01 + v._v.v._1*_m.m._11 + v._v.v._2*_m.m._21; @@ -693,6 +700,44 @@ typedef union { return t; } +// this = other1 * other2 +INLINE_LINMATH void FLOATNAME(LMatrix4):: +multiply(const FLOATNAME(LMatrix4) &other1, const FLOATNAME(LMatrix4) &other2) { +// faster than operator * since it writes result in place, avoiding extra copying +// this will fail if you try to mat.multiply(mat,other_mat) + + #ifdef _DEBUG + assert((&other1 != this) && (&other2 != this)); + #endif + + MATRIX4_PRODUCT((*this),other1,other2); +} + +// this = scale_mat(scale_vector) * other_mat, efficiently +INLINE_LINMATH void FLOATNAME(LMatrix4):: +scale_multiply(const FLOATNAME(LVecBase3) &scale_vector,const FLOATNAME(LMatrix4) &other_mat) { + #ifdef _DEBUG + assert(&other_mat != this); + #endif + + // optimize for 0.0 or 1.0 factors? + + _m.m._00 = other_mat._m.m._00 * scale_vector._v.v._0; + _m.m._01 = other_mat._m.m._01 * scale_vector._v.v._0; + _m.m._02 = other_mat._m.m._02 * scale_vector._v.v._0; + _m.m._03 = other_mat._m.m._03 * scale_vector._v.v._0; + + _m.m._10 = other_mat._m.m._10 * scale_vector._v.v._1; + _m.m._11 = other_mat._m.m._11 * scale_vector._v.v._1; + _m.m._12 = other_mat._m.m._12 * scale_vector._v.v._1; + _m.m._13 = other_mat._m.m._13 * scale_vector._v.v._1; + + _m.m._20 = other_mat._m.m._20 * scale_vector._v.v._2; + _m.m._21 = other_mat._m.m._21 * scale_vector._v.v._2; + _m.m._22 = other_mat._m.m._22 * scale_vector._v.v._2; + _m.m._23 = other_mat._m.m._23 * scale_vector._v.v._2; +} + //////////////////////////////////////////////////////////////////// // Function: LMatrix4::matrix * scalar // Access: Public @@ -733,7 +778,7 @@ operator * (FLOATTYPE scalar) const { INLINE_LINMATH FLOATNAME(LMatrix4) FLOATNAME(LMatrix4):: operator / (FLOATTYPE scalar) const { FLOATNAME(LMatrix4) t; - FLOATTYPE recip_scalar = 1.0/scalar; + FLOATTYPE recip_scalar = 1.0f/scalar; t._m.m._00 = _m.m._00 * recip_scalar; t._m.m._01 = _m.m._01 * recip_scalar; @@ -866,7 +911,7 @@ operator *= (FLOATTYPE scalar) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LMatrix4) &FLOATNAME(LMatrix4):: operator /= (FLOATTYPE scalar) { - FLOATTYPE recip_scalar = 1.0/scalar; + FLOATTYPE recip_scalar = 1.0f/scalar; _m.m._00 *= recip_scalar; _m.m._01 *= recip_scalar; _m.m._02 *= recip_scalar; @@ -960,10 +1005,10 @@ transpose_in_place() { // (transpose upper 3x3 and take negative of translation component) INLINE_LINMATH bool FLOATNAME(LMatrix4):: invert_from(const FLOATNAME(LMatrix4) &other) { - if (IS_NEARLY_EQUAL(other._m.m._30, 0.0) && - IS_NEARLY_EQUAL(other._m.m._31, 0.0) && - IS_NEARLY_EQUAL(other._m.m._32, 0.0) && - IS_NEARLY_EQUAL(other._m.m._33, 1.0)) { + if (IS_NEARLY_EQUAL(other._m.m._30, 0.0f) && + IS_NEARLY_EQUAL(other._m.m._31, 0.0f) && + IS_NEARLY_EQUAL(other._m.m._32, 0.0f) && + IS_NEARLY_EQUAL(other._m.m._33, 1.0f)) { return invert_affine_from(other); } @@ -1015,17 +1060,17 @@ invert_affine_from(const FLOATNAME(LMatrix4) &other) { rot.invert_from(other.get_upper_3()); // probably could use transpose here set_upper_3(rot); -// set_col(3, FLOATNAME(LVecBase4)(0.0, 0.0, 0.0, 1.0)); +// set_col(3, FLOATNAME(LVecBase4)(0.0f, 0.0f, 0.0f, 1.0f)); - _m.m._03 = 0.0; - _m.m._13 = 0.0; - _m.m._23 = 0.0; - _m.m._33 = 1.0; + _m.m._03 = 0.0f; + _m.m._13 = 0.0f; + _m.m._23 = 0.0f; + _m.m._33 = 1.0f; // compute -C*inv(A) /* for (int i = 0; i < 3; i++) { - (*this)(3, i) = 0.0; + (*this)(3, i) = 0.0f; for (int j = 0; j < 3; j++) { (*this)(3, i) -= other(3, j) * (*this)(j, i); } @@ -1068,10 +1113,10 @@ invert_in_place() { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LMatrix4) FLOATNAME(LMatrix4):: translate_mat(const FLOATNAME(LVecBase3) &trans) { - return FLOATNAME(LMatrix4)(1.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - trans._v.v._0, trans._v.v._1, trans._v.v._2, 1.0); + return FLOATNAME(LMatrix4)(1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, + trans._v.v._0, trans._v.v._1, trans._v.v._2, 1.0f); } //////////////////////////////////////////////////////////////////// @@ -1082,10 +1127,10 @@ translate_mat(const FLOATNAME(LVecBase3) &trans) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LMatrix4) FLOATNAME(LMatrix4):: translate_mat(FLOATTYPE tx, FLOATTYPE ty, FLOATTYPE tz) { - return FLOATNAME(LMatrix4)(1.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - tx, ty, tz, 1.0); + return FLOATNAME(LMatrix4)(1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, + tx, ty, tz, 1.0f); } //////////////////////////////////////////////////////////////////// @@ -1121,14 +1166,14 @@ rotate_mat(FLOATTYPE angle, FLOATNAME(LVecBase3) axis, // pass in something like (.5,.5,0). need to add flag parameter so caller // can request normalization if needed - if((cabs(axis_0)+cabs(axis_1)+cabs(axis_2)) != 1.0) { + if((cabs(axis_0)+cabs(axis_1)+cabs(axis_2)) != 1.0f) { */ FLOATTYPE length_sq = axis_0 * axis_0 + axis_1 * axis_1 + axis_2 * axis_2; #ifdef _DEBUG - nassertr(length_sq != 0.0, ident_mat()); + nassertr(length_sq != 0.0f, ident_mat()); #endif - FLOATTYPE recip_length = 1.0/csqrt(length_sq); + FLOATTYPE recip_length = 1.0f/csqrt(length_sq); axis_0 *= recip_length; axis_1 *= recip_length; @@ -1138,7 +1183,7 @@ rotate_mat(FLOATTYPE angle, FLOATNAME(LVecBase3) axis, FLOATTYPE angle_rad=deg_2_rad(angle); FLOATTYPE s,c; csincos(angle_rad,&s,&c); - FLOATTYPE t = 1.0 - c; + FLOATTYPE t = 1.0f - c; FLOATTYPE t0,t1,t2,s0,s1,s2; @@ -1161,14 +1206,14 @@ rotate_mat(FLOATTYPE angle, FLOATNAME(LVecBase3) axis, mat._m.m._21 = t2 * axis_1 - s0; mat._m.m._22 = t2 * axis_2 + c; - mat._m.m._03 = 0.0; - mat._m.m._13 = 0.0; - mat._m.m._23 = 0.0; + mat._m.m._03 = 0.0f; + mat._m.m._13 = 0.0f; + mat._m.m._23 = 0.0f; - mat._m.m._30 = 0.0; - mat._m.m._31 = 0.0; - mat._m.m._32 = 0.0; - mat._m.m._33 = 1.0; + mat._m.m._30 = 0.0f; + mat._m.m._31 = 0.0f; + mat._m.m._32 = 0.0f; + mat._m.m._33 = 1.0f; return mat; } @@ -1211,7 +1256,7 @@ rotate_mat_normaxis(FLOATTYPE angle, const FLOATNAME(LVecBase3) &axis, FLOATTYPE angle_rad=deg_2_rad(angle); FLOATTYPE s,c; csincos(angle_rad,&s,&c); - FLOATTYPE t = 1.0 - c; + FLOATTYPE t = 1.0f - c; FLOATTYPE t0,t1,t2,s0,s1,s2; @@ -1234,14 +1279,14 @@ rotate_mat_normaxis(FLOATTYPE angle, const FLOATNAME(LVecBase3) &axis, result_mat._m.m._21 = t2 * axis_1 - s0; result_mat._m.m._22 = t2 * axis_2 + c; - result_mat._m.m._03 = 0.0; - result_mat._m.m._13 = 0.0; - result_mat._m.m._23 = 0.0; + result_mat._m.m._03 = 0.0f; + result_mat._m.m._13 = 0.0f; + result_mat._m.m._23 = 0.0f; - result_mat._m.m._30 = 0.0; - result_mat._m.m._31 = 0.0; - result_mat._m.m._32 = 0.0; - result_mat._m.m._33 = 1.0; + result_mat._m.m._30 = 0.0f; + result_mat._m.m._31 = 0.0f; + result_mat._m.m._32 = 0.0f; + result_mat._m.m._33 = 1.0f; } //////////////////////////////////////////////////////////////////// @@ -1252,10 +1297,10 @@ rotate_mat_normaxis(FLOATTYPE angle, const FLOATNAME(LVecBase3) &axis, //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LMatrix4) FLOATNAME(LMatrix4):: scale_mat(const FLOATNAME(LVecBase3) &scale) { - return FLOATNAME(LMatrix4)(scale._v.v._0, 0.0, 0.0, 0.0, - 0.0, scale._v.v._1, 0.0, 0.0, - 0.0, 0.0, scale._v.v._2, 0.0, - 0.0, 0.0, 0.0, 1.0); + return FLOATNAME(LMatrix4)(scale._v.v._0, 0.0f, 0.0f, 0.0f, + 0.0f, scale._v.v._1, 0.0f, 0.0f, + 0.0f, 0.0f, scale._v.v._2, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f); } @@ -1267,13 +1312,13 @@ scale_mat(const FLOATNAME(LVecBase3) &scale) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LMatrix4) FLOATNAME(LMatrix4):: scale_mat(FLOATTYPE sx, FLOATTYPE sy, FLOATTYPE sz) { - return FLOATNAME(LMatrix4)(sx, 0.0, 0.0, 0.0, - 0.0, sy, 0.0, 0.0, - 0.0, 0.0, sz, 0.0, - 0.0, 0.0, 0.0, 1.0); + return FLOATNAME(LMatrix4)( + sx, 0.0f, 0.0f, 0.0f, + 0.0f, sy, 0.0f, 0.0f, + 0.0f, 0.0f, sz, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f); } - //////////////////////////////////////////////////////////////////// // Function: LMatrix::scale_mat // Access: Public, Static @@ -1282,13 +1327,13 @@ scale_mat(FLOATTYPE sx, FLOATTYPE sy, FLOATTYPE sz) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LMatrix4) FLOATNAME(LMatrix4):: scale_mat(FLOATTYPE scale) { - return FLOATNAME(LMatrix4)(scale, 0.0, 0.0, 0.0, - 0.0, scale, 0.0, 0.0, - 0.0, 0.0, scale, 0.0, - 0.0, 0.0, 0.0, 1.0); + return FLOATNAME(LMatrix4)( + scale, 0.0f, 0.0f, 0.0f, + 0.0f, scale, 0.0f, 0.0f, + 0.0f, 0.0f, scale, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f); } - //////////////////////////////////////////////////////////////////// // Function: LMatrix::y_to_z_up_mat // Access: Public, Static @@ -1332,8 +1377,6 @@ generate_hash(ChecksumHashGenerator &hash) const { generate_hash(hash, NEARLY_ZERO(FLOATTYPE)); } - - //////////////////////////////////////////////////////////////////// // Function: transpose // Description: Transposes the given matrix and returns it. @@ -1356,3 +1399,4 @@ invert(const FLOATNAME(LMatrix4) &a) { nassertr(nonsingular, FLOATNAME(LMatrix4)::ident_mat()); return result; } + diff --git a/panda/src/linmath/lmatrix4_src.cxx b/panda/src/linmath/lmatrix4_src.cxx index 58880328f4..298c2d0730 100644 --- a/panda/src/linmath/lmatrix4_src.cxx +++ b/panda/src/linmath/lmatrix4_src.cxx @@ -6,22 +6,22 @@ TypeHandle FLOATNAME(LMatrix4)::_type_handle; const FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::_ident_mat = - FLOATNAME(LMatrix4)(1.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 1.0); + FLOATNAME(LMatrix4)(1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f); const FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::_y_to_z_up_mat = - FLOATNAME(LMatrix4)(1.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - 0.0,-1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0); + FLOATNAME(LMatrix4)(1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, + 0.0f,-1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f); const FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::_z_to_y_up_mat = - FLOATNAME(LMatrix4)(1.0, 0.0, 0.0, 0.0, - 0.0, 0.0,-1.0, 0.0, - 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0); + FLOATNAME(LMatrix4)(1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f,-1.0f, 0.0f, + 0.0f, 1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f); //////////////////////////////////////////////////////////////////// // Function: LMatrix::convert_mat @@ -42,8 +42,8 @@ convert_mat(CoordinateSystem from, CoordinateSystem to) { switch (to) { case CS_zup_left: return ident_mat(); case CS_yup_left: return z_to_y_up_mat(); - case CS_zup_right: return scale_mat(1.0, -1.0, 1.0); - case CS_yup_right: return scale_mat(1.0, -1.0, 1.0) * z_to_y_up_mat(); + case CS_zup_right: return scale_mat(1.0f, -1.0f, 1.0f); + case CS_yup_right: return scale_mat(1.0f, -1.0f, 1.0f) * z_to_y_up_mat(); default: break; } break; @@ -52,16 +52,16 @@ convert_mat(CoordinateSystem from, CoordinateSystem to) { switch (to) { case CS_zup_left: return y_to_z_up_mat(); case CS_yup_left: return ident_mat(); - case CS_zup_right: return scale_mat(1.0, 1.0, -1.0) * y_to_z_up_mat(); - case CS_yup_right: return scale_mat(1.0, 1.0, -1.0); + case CS_zup_right: return scale_mat(1.0f, 1.0f, -1.0f) * y_to_z_up_mat(); + case CS_yup_right: return scale_mat(1.0f, 1.0f, -1.0f); default: break; } break; case CS_zup_right: switch (to) { - case CS_zup_left: return scale_mat(1.0, -1.0, 1.0); - case CS_yup_left: return scale_mat(1.0, -1.0, 1.0) * z_to_y_up_mat(); + case CS_zup_left: return scale_mat(1.0f, -1.0f, 1.0f); + case CS_yup_left: return scale_mat(1.0f, -1.0f, 1.0f) * z_to_y_up_mat(); case CS_zup_right: return ident_mat(); case CS_yup_right: return z_to_y_up_mat(); default: break; @@ -70,8 +70,8 @@ convert_mat(CoordinateSystem from, CoordinateSystem to) { case CS_yup_right: switch (to) { - case CS_zup_left: return scale_mat(1.0, 1.0, -1.0) * y_to_z_up_mat(); - case CS_yup_left: return scale_mat(1.0, 1.0, -1.0); + case CS_zup_left: return scale_mat(1.0f, 1.0f, -1.0f) * y_to_z_up_mat(); + case CS_yup_left: return scale_mat(1.0f, 1.0f, -1.0f); case CS_zup_right: return y_to_z_up_mat(); case CS_yup_right: return ident_mat(); default: break; @@ -221,7 +221,7 @@ decompose_mat(int index[4]) { int i, j, k; FLOATTYPE vv[4]; for (i = 0; i < 4; i++) { - FLOATTYPE big = 0.0; + FLOATTYPE big = 0.0f; for (j = 0; j < 4; j++) { FLOATTYPE temp = fabs((*this)(i,j)); if (temp > big) { @@ -232,7 +232,7 @@ decompose_mat(int index[4]) { if (IS_NEARLY_ZERO(big)) { return false; } - vv[i] = 1.0 / big; + vv[i] = 1.0f / big; } for (j = 0; j < 4; j++) { @@ -244,7 +244,7 @@ decompose_mat(int index[4]) { (*this)(i,j) = sum; } - FLOATTYPE big = 0.0; + FLOATTYPE big = 0.0f; int imax = -1; for (i = j; i < 4; i++) { FLOATTYPE sum = (*this)(i,j); @@ -270,12 +270,12 @@ decompose_mat(int index[4]) { } index[j] = imax; - if ((*this)(j,j) == 0.0) { + if ((*this)(j,j) == 0.0f) { (*this)(j,j) = NEARLY_ZERO(FLOATTYPE); } if (j != 4 - 1) { - FLOATTYPE dum = 1.0 / (*this)(j,j); + FLOATTYPE dum = 1.0f / (*this)(j,j); for (i = j + 1; i < 4; i++) { (*this)(i,j) *= dum; } diff --git a/panda/src/linmath/lmatrix4_src.h b/panda/src/linmath/lmatrix4_src.h index 2333a00e47..3ce3d061bf 100644 --- a/panda/src/linmath/lmatrix4_src.h +++ b/panda/src/linmath/lmatrix4_src.h @@ -48,6 +48,11 @@ PUBLISHED: INLINE_LINMATH FLOATNAME(LVecBase4) get_col(int col) const; INLINE_LINMATH FLOATNAME(LVecBase3) get_row3(int row) const; + + // these versions inline better + INLINE_LINMATH void get_row(FLOATNAME(LVecBase4) &result_vec, int row) const; + INLINE_LINMATH void get_row3(FLOATNAME(LVecBase3) &result_vec, int row) const; + INLINE_LINMATH FLOATNAME(LVecBase3) get_col3(int col) const; INLINE_LINMATH FLOATTYPE &operator () (int row, int col); @@ -83,7 +88,14 @@ PUBLISHED: INLINE_LINMATH FLOATNAME(LVecBase3) xform_vec(const FLOATNAME(LVecBase3) &v) const; + // this = other1 * other2 + INLINE_LINMATH void multiply(const FLOATNAME(LMatrix4) &other1, const FLOATNAME(LMatrix4) &other2); + + // this = scale_mat(scale_vector) * other_mat, efficiently + INLINE_LINMATH void scale_multiply(const FLOATNAME(LVecBase3) &scale_vector,const FLOATNAME(LMatrix4) &other_mat); + INLINE_LINMATH FLOATNAME(LMatrix4) operator * (const FLOATNAME(LMatrix4) &other) const; + INLINE_LINMATH FLOATNAME(LMatrix4) operator * (FLOATTYPE scalar) const; INLINE_LINMATH FLOATNAME(LMatrix4) operator / (FLOATTYPE scalar) const; @@ -147,7 +159,7 @@ public: public: union { struct { - FLOATTYPE _00, _01, _02, _03; + FLOATTYPE _00, _01, _02, _03; FLOATTYPE _10, _11, _12, _13; FLOATTYPE _20, _21, _22, _23; FLOATTYPE _30, _31, _32, _33; diff --git a/panda/src/linmath/lorientation_src.I b/panda/src/linmath/lorientation_src.I index 693b1df825..1a56d9aece 100644 --- a/panda/src/linmath/lorientation_src.I +++ b/panda/src/linmath/lorientation_src.I @@ -39,8 +39,8 @@ FLOATNAME(LOrientation)(FLOATTYPE r, FLOATTYPE i, FLOATTYPE j, FLOATTYPE k) : //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LOrientation):: FLOATNAME(LOrientation)(const FLOATNAME(LVector3) &point_at, float twist) { - float radians = twist * ((float) MathNumbers::pi / 180.0f); - float theta_over_2 = radians / 2.0f; + float radians = deg_2_rad(twist); + float theta_over_2 = radians * FLOATCONST(0.5); float sin_to2 = sinf(theta_over_2); set_r(cosf(theta_over_2)); diff --git a/panda/src/linmath/lpoint3_src.I b/panda/src/linmath/lpoint3_src.I index c6d90a7cef..466bde0319 100644 --- a/panda/src/linmath/lpoint3_src.I +++ b/panda/src/linmath/lpoint3_src.I @@ -206,7 +206,7 @@ operator / (FLOATTYPE scalar) const { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LPoint3) FLOATNAME(LPoint3):: origin(CoordinateSystem) { - return FLOATNAME(LPoint3)(0.0, 0.0, 0.0); + return FLOATNAME(LPoint3)(0.0f, 0.0f, 0.0f); } //////////////////////////////////////////////////////////////////// diff --git a/panda/src/linmath/lquaternion_src.I b/panda/src/linmath/lquaternion_src.I index 14e391461b..4bfcacc5a0 100644 --- a/panda/src/linmath/lquaternion_src.I +++ b/panda/src/linmath/lquaternion_src.I @@ -226,11 +226,11 @@ set_k(FLOATTYPE k) { INLINE_LINMATH bool FLOATNAME(LQuaternion):: normalize() { FLOATTYPE l2 = (*this).dot(*this); - if (l2 == (FLOATTYPE)0.0) { - set(0.0, 0.0, 0.0, 0.0); + if (l2 == (FLOATTYPE)0.0f) { + set(0.0f, 0.0f, 0.0f, 0.0f); return false; - } else if (!IS_THRESHOLD_EQUAL(l2, 1.0, NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE))) { + } else if (!IS_THRESHOLD_EQUAL(l2, 1.0f, NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE))) { (*this) /= csqrt(l2); } diff --git a/panda/src/linmath/lquaternion_src.cxx b/panda/src/linmath/lquaternion_src.cxx index 6d0c268301..4465f35508 100644 --- a/panda/src/linmath/lquaternion_src.cxx +++ b/panda/src/linmath/lquaternion_src.cxx @@ -2,7 +2,7 @@ TypeHandle FLOATNAME(LQuaternion)::_type_handle; const FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::_ident_quat = - FLOATNAME(LQuaternion)(1.0, 0.0, 0.0, 0.0); + FLOATNAME(LQuaternion)(1.0f, 0.0f, 0.0f, 0.0f); //////////////////////////////////////////////////////////////////// // Function: FLOATNAME(LQuaternion)::pure_imaginary_quat diff --git a/panda/src/linmath/lrotation_src.I b/panda/src/linmath/lrotation_src.I index 246e59da8a..54adccab86 100644 --- a/panda/src/linmath/lrotation_src.I +++ b/panda/src/linmath/lrotation_src.I @@ -59,8 +59,8 @@ FLOATNAME(LRotation)(const FLOATNAME(LMatrix4) &m) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LRotation):: FLOATNAME(LRotation)(const FLOATNAME(LVector3) &axis, FLOATTYPE angle) { - FLOATTYPE radians = angle * ((FLOATTYPE) MathNumbers::pi / (FLOATTYPE)180.0); - FLOATTYPE theta_over_2 = radians / (FLOATTYPE)2.0; + FLOATTYPE radians = deg_2_rad(angle); + FLOATTYPE theta_over_2 = radians * FLOATCONST(0.5); FLOATTYPE sin_to2 = csin(theta_over_2); set_r(ccos(theta_over_2)); diff --git a/panda/src/linmath/luse.h b/panda/src/linmath/luse.h index de91dc8308..eb30a3895a 100644 --- a/panda/src/linmath/luse.h +++ b/panda/src/linmath/luse.h @@ -61,6 +61,9 @@ #include "lorientation.h" #include "lcast_to.h" +//ensure FLOATTYPE is set to float for macros are used outside of LINMATH +#include "fltnames.h" + // This macro defines the cast-to-another-numeric-type operator for // all of the things defined in this package. It works by virtue of // there being an appropriate lcast_to() template function defined for diff --git a/panda/src/linmath/lvecBase2_src.I b/panda/src/linmath/lvecBase2_src.I index 3caf5ea936..1a41d6b25e 100644 --- a/panda/src/linmath/lvecBase2_src.I +++ b/panda/src/linmath/lvecBase2_src.I @@ -421,7 +421,7 @@ operator * (FLOATTYPE scalar) const { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LVecBase2) FLOATNAME(LVecBase2):: operator / (FLOATTYPE scalar) const { - FLOATTYPE recip_scalar = 1.0/scalar; + FLOATTYPE recip_scalar = 1.0f/scalar; return FLOATNAME(LVecBase2)(_v.v._0 * recip_scalar, _v.v._1 * recip_scalar); } @@ -466,7 +466,7 @@ operator *= (FLOATTYPE scalar) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH void FLOATNAME(LVecBase2):: operator /= (FLOATTYPE scalar) { - FLOATTYPE recip_scalar = 1.0/scalar; + FLOATTYPE recip_scalar = 1.0f/scalar; _v.v._0 *= recip_scalar; _v.v._1 *= recip_scalar; } diff --git a/panda/src/linmath/lvecBase2_src.cxx b/panda/src/linmath/lvecBase2_src.cxx index 2f17f901f5..5a83110ed3 100644 --- a/panda/src/linmath/lvecBase2_src.cxx +++ b/panda/src/linmath/lvecBase2_src.cxx @@ -6,11 +6,11 @@ TypeHandle FLOATNAME(LVecBase2)::_type_handle; const FLOATNAME(LVecBase2) FLOATNAME(LVecBase2)::_zero = - FLOATNAME(LVecBase2)(0.0, 0.0); + FLOATNAME(LVecBase2)(0.0f, 0.0f); const FLOATNAME(LVecBase2) FLOATNAME(LVecBase2)::_unit_x = - FLOATNAME(LVecBase2)(1.0, 0.0); + FLOATNAME(LVecBase2)(1.0f, 0.0f); const FLOATNAME(LVecBase2) FLOATNAME(LVecBase2)::_unit_y = - FLOATNAME(LVecBase2)(0.0, 1.0); + FLOATNAME(LVecBase2)(0.0f, 1.0f); //////////////////////////////////////////////////////////////////// // Function: LVecBase2::init_type diff --git a/panda/src/linmath/lvecBase3_src.I b/panda/src/linmath/lvecBase3_src.I index 579267f72b..2747faf415 100644 --- a/panda/src/linmath/lvecBase3_src.I +++ b/panda/src/linmath/lvecBase3_src.I @@ -474,9 +474,10 @@ operator * (FLOATTYPE scalar) const { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LVecBase3) FLOATNAME(LVecBase3):: operator / (FLOATTYPE scalar) const { - return FLOATNAME(LVecBase3)(_v.v._0 / scalar, - _v.v._1 / scalar, - _v.v._2 / scalar); + FLOATTYPE recip_scalar = 1.0f/scalar; + return FLOATNAME(LVecBase3)(_v.v._0 * recip_scalar, + _v.v._1 * recip_scalar, + _v.v._2 * recip_scalar); } //////////////////////////////////////////////////////////////////// @@ -522,7 +523,7 @@ operator *= (FLOATTYPE scalar) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH void FLOATNAME(LVecBase3):: operator /= (FLOATTYPE scalar) { - FLOATTYPE recip_scalar = 1.0/scalar; + FLOATTYPE recip_scalar = 1.0f/scalar; _v.v._0 *= recip_scalar; _v.v._1 *= recip_scalar; _v.v._2 *= recip_scalar; diff --git a/panda/src/linmath/lvecBase3_src.cxx b/panda/src/linmath/lvecBase3_src.cxx index f74d349ac9..900e96b250 100644 --- a/panda/src/linmath/lvecBase3_src.cxx +++ b/panda/src/linmath/lvecBase3_src.cxx @@ -6,13 +6,13 @@ TypeHandle FLOATNAME(LVecBase3)::_type_handle; const FLOATNAME(LVecBase3) FLOATNAME(LVecBase3)::_zero = - FLOATNAME(LVecBase3)(0.0, 0.0, 0.0); + FLOATNAME(LVecBase3)(0.0f, 0.0f, 0.0f); const FLOATNAME(LVecBase3) FLOATNAME(LVecBase3)::_unit_x = - FLOATNAME(LVecBase3)(1.0, 0.0, 0.0); + FLOATNAME(LVecBase3)(1.0f, 0.0f, 0.0f); const FLOATNAME(LVecBase3) FLOATNAME(LVecBase3)::_unit_y = - FLOATNAME(LVecBase3)(0.0, 1.0, 0.0); + FLOATNAME(LVecBase3)(0.0f, 1.0f, 0.0f); const FLOATNAME(LVecBase3) FLOATNAME(LVecBase3)::_unit_z = - FLOATNAME(LVecBase3)(0.0, 0.0, 1.0); + FLOATNAME(LVecBase3)(0.0f, 0.0f, 1.0f); //////////////////////////////////////////////////////////////////// // Function: LVecBase3::init_type diff --git a/panda/src/linmath/lvecBase4_src.I b/panda/src/linmath/lvecBase4_src.I index 14b9202b8c..6ff2a18b33 100644 --- a/panda/src/linmath/lvecBase4_src.I +++ b/panda/src/linmath/lvecBase4_src.I @@ -507,7 +507,7 @@ operator * (FLOATTYPE scalar) const { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LVecBase4) FLOATNAME(LVecBase4):: operator / (FLOATTYPE scalar) const { - FLOATTYPE recip_scalar = 1.0/scalar; + FLOATTYPE recip_scalar = 1.0f/scalar; return FLOATNAME(LVecBase4)(_v.v._0 * recip_scalar, _v.v._1 * recip_scalar, _v.v._2 * recip_scalar, @@ -560,7 +560,7 @@ operator *= (FLOATTYPE scalar) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH void FLOATNAME(LVecBase4):: operator /= (FLOATTYPE scalar) { - FLOATTYPE recip_scalar = 1.0/scalar; + FLOATTYPE recip_scalar = 1.0f/scalar; _v.v._0 *= recip_scalar; _v.v._1 *= recip_scalar; _v.v._2 *= recip_scalar; diff --git a/panda/src/linmath/lvecBase4_src.cxx b/panda/src/linmath/lvecBase4_src.cxx index b5818617b3..7a126fa4f0 100644 --- a/panda/src/linmath/lvecBase4_src.cxx +++ b/panda/src/linmath/lvecBase4_src.cxx @@ -6,15 +6,15 @@ TypeHandle FLOATNAME(LVecBase4)::_type_handle; const FLOATNAME(LVecBase4) FLOATNAME(LVecBase4)::_zero = - FLOATNAME(LVecBase4)(0.0, 0.0, 0.0, 0.0); + FLOATNAME(LVecBase4)(0.0f, 0.0f, 0.0f, 0.0f); const FLOATNAME(LVecBase4) FLOATNAME(LVecBase4)::_unit_x = - FLOATNAME(LVecBase4)(1.0, 0.0, 0.0, 0.0); + FLOATNAME(LVecBase4)(1.0f, 0.0f, 0.0f, 0.0f); const FLOATNAME(LVecBase4) FLOATNAME(LVecBase4)::_unit_y = - FLOATNAME(LVecBase4)(0.0, 1.0, 0.0, 0.0); + FLOATNAME(LVecBase4)(0.0f, 1.0f, 0.0f, 0.0f); const FLOATNAME(LVecBase4) FLOATNAME(LVecBase4)::_unit_z = - FLOATNAME(LVecBase4)(0.0, 0.0, 1.0, 0.0); + FLOATNAME(LVecBase4)(0.0f, 0.0f, 1.0f, 0.0f); const FLOATNAME(LVecBase4) FLOATNAME(LVecBase4)::_unit_w = - FLOATNAME(LVecBase4)(0.0, 0.0, 0.0, 1.0); + FLOATNAME(LVecBase4)(0.0f, 0.0f, 0.0f, 1.0f); //////////////////////////////////////////////////////////////////// // Function: LVecBase4::init_type diff --git a/panda/src/linmath/lvector2_src.I b/panda/src/linmath/lvector2_src.I index 1f0cdf32dd..7517d6893e 100644 --- a/panda/src/linmath/lvector2_src.I +++ b/panda/src/linmath/lvector2_src.I @@ -177,11 +177,11 @@ length_squared() const { INLINE_LINMATH bool FLOATNAME(LVector2):: normalize() { FLOATTYPE l2 = length_squared(); - if (l2 == (FLOATTYPE)0.0) { - set(0.0, 0.0); + if (l2 == (FLOATTYPE)0.0f) { + set(0.0f, 0.0f); return false; - } else if (!IS_THRESHOLD_EQUAL(l2, 1.0, NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE))) { + } else if (!IS_THRESHOLD_EQUAL(l2, 1.0f, NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE))) { (*this) /= csqrt(l2); } @@ -205,6 +205,6 @@ operator * (FLOATTYPE scalar) const { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LVector2) FLOATNAME(LVector2):: operator / (FLOATTYPE scalar) const { - FLOATTYPE recip_scalar = 1.0/scalar; + FLOATTYPE recip_scalar = 1.0f/scalar; return FLOATNAME(LVector2)(FLOATNAME(LVecBase2)::operator * (recip_scalar)); } diff --git a/panda/src/linmath/lvector3_src.I b/panda/src/linmath/lvector3_src.I index a55bb890e8..507a26aeb6 100644 --- a/panda/src/linmath/lvector3_src.I +++ b/panda/src/linmath/lvector3_src.I @@ -188,11 +188,11 @@ length_squared() const { INLINE_LINMATH bool FLOATNAME(LVector3):: normalize() { FLOATTYPE l2 = length_squared(); - if (l2 == (FLOATTYPE)0.0) { - set(0.0, 0.0, 0.0); + if (l2 == (FLOATTYPE)0.0f) { + set(0.0f, 0.0f, 0.0f); return false; - } else if (!IS_THRESHOLD_EQUAL(l2, 1.0, NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE))) { + } else if (!IS_THRESHOLD_EQUAL(l2, 1.0f, (NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE)))) { (*this) /= csqrt(l2); } @@ -226,7 +226,7 @@ operator * (FLOATTYPE scalar) const { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LVector3) FLOATNAME(LVector3):: operator / (FLOATTYPE scalar) const { - FLOATTYPE recip_scalar = 1.0/scalar; + FLOATTYPE recip_scalar = 1.0f/scalar; return FLOATNAME(LVector3)(FLOATNAME(LVecBase3)::operator * (recip_scalar)); } @@ -244,16 +244,16 @@ up(CoordinateSystem cs) { switch (cs) { case CS_zup_right: case CS_zup_left: - return FLOATNAME(LVector3)(0.0, 0.0, 1.0); + return FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f); case CS_yup_right: case CS_yup_left: - return FLOATNAME(LVector3)(0.0, 1.0, 0.0); + return FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f); default: linmath_cat.error() << "Invalid coordinate system!\n"; - return FLOATNAME(LVector3)(0.0, 0.0, 0.0); + return FLOATNAME(LVector3)(0.0f, 0.0f, 0.0f); } } @@ -265,7 +265,7 @@ up(CoordinateSystem cs) { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LVector3) FLOATNAME(LVector3):: right(CoordinateSystem) { - return FLOATNAME(LVector3)(1.0, 0.0, 0.0); + return FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f); } //////////////////////////////////////////////////////////////////// @@ -281,21 +281,21 @@ forward(CoordinateSystem cs) { } switch (cs) { case CS_zup_right: - return FLOATNAME(LVector3)(0.0, 1.0, 0.0); + return FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f); case CS_zup_left: - return FLOATNAME(LVector3)(0.0, -1.0, 0.0); + return FLOATNAME(LVector3)(0.0f, -1.0f, 0.0f); case CS_yup_right: - return FLOATNAME(LVector3)(0.0, 0.0, -1.0); + return FLOATNAME(LVector3)(0.0f, 0.0f, -1.0f); case CS_yup_left: - return FLOATNAME(LVector3)(0.0, 0.0, 1.0); + return FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f); default: linmath_cat.error() << "Invalid coordinate system!\n"; - return FLOATNAME(LVector3)(0.0, 0.0, 0.0); + return FLOATNAME(LVector3)(0.0f, 0.0f, 0.0f); } } @@ -377,7 +377,7 @@ rfu(FLOATTYPE right_v, FLOATTYPE fwd_v, FLOATTYPE up_v, default: linmath_cat.error() << "Invalid coordinate system!\n"; - return FLOATNAME(LVector3)(0.0, 0.0, 0.0); + return FLOATNAME(LVector3)(0.0f, 0.0f, 0.0f); } return FLOATNAME(LVector3)(right_v,vy,vz); diff --git a/panda/src/linmath/lvector4_src.I b/panda/src/linmath/lvector4_src.I index 9a4412ffc6..e709e85b21 100644 --- a/panda/src/linmath/lvector4_src.I +++ b/panda/src/linmath/lvector4_src.I @@ -198,11 +198,11 @@ length_squared() const { INLINE_LINMATH bool FLOATNAME(LVector4):: normalize() { FLOATTYPE l2 = length_squared(); - if (l2 == (FLOATTYPE)0.0) { - set(0.0, 0.0, 0.0, 0.0); + if (l2 == (FLOATTYPE)0.0f) { + set(0.0f, 0.0f, 0.0f, 0.0f); return false; - } else if (!IS_THRESHOLD_EQUAL(l2, 1.0, NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE))) { + } else if (!IS_THRESHOLD_EQUAL(l2, 1.0f, NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE))) { (*this) /= csqrt(l2); } @@ -226,6 +226,6 @@ operator * (FLOATTYPE scalar) const { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATNAME(LVector4) FLOATNAME(LVector4):: operator / (FLOATTYPE scalar) const { - FLOATTYPE recip_scalar = 1.0/scalar; + FLOATTYPE recip_scalar = 1.0f/scalar; return FLOATNAME(LVector4)(FLOATNAME(LVecBase4)::operator * (recip_scalar)); } diff --git a/panda/src/linmath/mathNumbers.cxx b/panda/src/linmath/mathNumbers.cxx index 1788a6e080..de725cfe5c 100644 --- a/panda/src/linmath/mathNumbers.cxx +++ b/panda/src/linmath/mathNumbers.cxx @@ -6,5 +6,8 @@ #include "mathNumbers.h" #include -const double MathNumbers::pi = 4.0 * atan(1); -const double MathNumbers::ln2 = log(2); +const double MathNumbers::pi = 4.0 * atan(1.0); +const double MathNumbers::ln2 = log(2.0); + +const float MathNumbers::pi_f = 4.0 * atan(1.0); +const float MathNumbers::ln2_f = log(2.0); diff --git a/panda/src/linmath/mathNumbers.h b/panda/src/linmath/mathNumbers.h index c71eaf11f5..5d42efc476 100644 --- a/panda/src/linmath/mathNumbers.h +++ b/panda/src/linmath/mathNumbers.h @@ -10,6 +10,9 @@ class EXPCL_PANDA MathNumbers { PUBLISHED: + static const float pi_f; + static const float ln2_f; + static const double pi; static const double ln2; }; diff --git a/panda/src/linmath/test_math.cxx b/panda/src/linmath/test_math.cxx index 2fd7c4aa83..c026131395 100644 --- a/panda/src/linmath/test_math.cxx +++ b/panda/src/linmath/test_math.cxx @@ -47,7 +47,7 @@ int main(int argc, char *argv[]) { /* LMatrix4d x = LMatrix4d::ident_mat(); LMatrix4d y = LMatrix4d::rotate_mat(90.0, LVector3d::up()); - LMatrix4d a = LMatrix4d::translate_mat(10.0, 10.0, 0.0); + LMatrix4d a = LMatrix4d::translate_mat(10.0, 10.0, 0.0f); nout << "x is " << x << "\ny is " << y << "\n" << "x * y is " << x * y << "\n" @@ -73,14 +73,14 @@ int main(int argc, char *argv[]) { << "r * invert(y) is " << r * invert(y) << "\n" << "r * a is " << r * a << "\n"; - LPoint3d p(0.0, 1.0, 1.0); + LPoint3d p(0.0f, 1.0f, 1.0f); nout << "p is " << p << "\n" << "p * x is " << p * x << "\n" << "p * y is " << p * y << "\n" << "p * invert(y) is " << p * invert(y) << "\n" << "p * a is " << p * a << "\n"; - LVecBase4d q(0.0, 1.0, 1.0, 1.0); + LVecBase4d q(0.0f, 1.0f, 1.0f, 1.0f); nout << "q is " << q << "\n" << "q * x is " << q * x << "\n" << "q * y is " << q * y << "\n" @@ -102,8 +102,8 @@ int main(int argc, char *argv[]) { << "\n"; { - LVecBase3f hpr(0.0, 0.0, 0.0); - LVecBase3f scale(1.0, 1.0, 1.0); + LVecBase3f hpr(0.0f, 0.0f, 0.0f); + LVecBase3f scale(1.0f, 1.0f, 1.0f); if (argc > 3) { hpr.set(atof(argv[1]), atof(argv[2]), atof(argv[3]));