add rotate_mat_normaxis

This commit is contained in:
cxgeorge 2001-05-02 00:05:13 +00:00
parent 5f50a0e81c
commit efd47a12f1
10 changed files with 185 additions and 40 deletions

View File

@ -263,13 +263,13 @@ void ChanEval(GraphicsWindow* win, WindowItem& W, LayoutItem& L, SVec& S,
case SetupItem::Up:
break;
case SetupItem::Down:
orient = new TransformTransition(LMatrix4f::rotate_mat(180., LVector3f::forward()));
orient = new TransformTransition(LMatrix4f::rotate_mat_normaxis(180., LVector3f::forward()));
break;
case SetupItem::Left:
orient = new TransformTransition(LMatrix4f::rotate_mat(90., LVector3f::forward()));
orient = new TransformTransition(LMatrix4f::rotate_mat_normaxis(90., LVector3f::forward()));
break;
case SetupItem::Right:
orient = new TransformTransition(LMatrix4f::rotate_mat(-90., LVector3f::forward()));
orient = new TransformTransition(LMatrix4f::rotate_mat_normaxis(-90., LVector3f::forward()));
break;
}

View File

@ -198,18 +198,18 @@ compose_with_order(LMatrix4d &mat,
break;
case 'h':
mat = mat * LMatrix4d::rotate_mat(hpr[0], LVector3d::up(cs), cs);
mat = mat * LMatrix4d::rotate_mat_normaxis(hpr[0], LVector3d::up(cs), cs);
break;
case 'p':
mat = mat * LMatrix4d::rotate_mat(hpr[1], LVector3d::right(cs), cs);
mat = mat * LMatrix4d::rotate_mat_normaxis(hpr[1], LVector3d::right(cs), cs);
break;
case 'r':
if (reverse_roll) {
mat = mat * LMatrix4d::rotate_mat(-hpr[2], LVector3d::forward(cs), cs);
mat = mat * LMatrix4d::rotate_mat_normaxis(-hpr[2], LVector3d::forward(cs), cs);
} else {
mat = mat * LMatrix4d::rotate_mat(hpr[2], LVector3d::forward(cs), cs);
mat = mat * LMatrix4d::rotate_mat_normaxis(hpr[2], LVector3d::forward(cs), cs);
}
break;

View File

@ -1103,19 +1103,19 @@ translate_3d: TRANSLATE '{' real real real '}'
rotx_3d: ROTX '{' real '}'
{
matrix_3d *= LMatrix4d::rotate_mat($3, LVector3d(1.0, 0.0, 0.0));
matrix_3d *= LMatrix4d::rotate_mat_normaxis($3, LVector3d(1.0, 0.0, 0.0));
}
;
roty_3d: ROTY '{' real '}'
{
matrix_3d *= LMatrix4d::rotate_mat($3, LVector3d(0.0, 1.0, 0.0));
matrix_3d *= LMatrix4d::rotate_mat_normaxis($3, LVector3d(0.0, 1.0, 0.0));
}
;
rotz_3d: ROTZ '{' real '}'
{
matrix_3d *= LMatrix4d::rotate_mat($3, LVector3d(0.0, 0.0, 1.0));
matrix_3d *= LMatrix4d::rotate_mat_normaxis($3, LVector3d(0.0, 0.0, 1.0));
}
;

View File

@ -16,15 +16,15 @@ compose_matrix(FLOATNAME(LMatrix3) &mat,
if (temp_hpr_fix) {
mat =
FLOATNAME(LMatrix3)::scale_mat(scale) *
FLOATNAME(LMatrix3)::rotate_mat(hpr[2], FLOATNAME(LVector3)::forward(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat(hpr[1], FLOATNAME(LVector3)::right(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat(hpr[0], FLOATNAME(LVector3)::up(cs), cs);
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);
} else {
mat =
FLOATNAME(LMatrix3)::scale_mat(scale) *
FLOATNAME(LMatrix3)::rotate_mat(hpr[1], FLOATNAME(LVector3)::right(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat(hpr[0], FLOATNAME(LVector3)::up(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat(hpr[2], FLOATNAME(LVector3)::back(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[2], FLOATNAME(LVector3)::back(cs), cs);
}
}
@ -58,7 +58,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
// Unwind the heading, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat(-heading, FLOATNAME(LVector3)(0.0, 1.0, 0.0),
rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0, 1.0, 0.0),
CS_yup_right);
x = x * rot_y;
@ -74,7 +74,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0),
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0),
CS_yup_right);
x = x * rot_x;
@ -90,7 +90,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
// Unwind the roll from the axes, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat(-roll, FLOATNAME(LVector3)(0.0, 0.0, 1.0),
rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0, 0.0, 1.0),
CS_yup_right);
x = x * rot_z;
@ -124,7 +124,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
// Unwind the roll from the axes, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat(-roll, FLOATNAME(LVector3)(0.0, 0.0, 1.0),
rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0, 0.0, 1.0),
CS_yup_right);
x = x * rot_z;
@ -141,7 +141,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
// Unwind the heading, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat(-heading, FLOATNAME(LVector3)(0.0, 1.0, 0.0),
rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0, 1.0, 0.0),
CS_yup_right);
x = x * rot_y;
@ -157,7 +157,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0),
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0),
CS_yup_right);
x = x * rot_x;
@ -204,7 +204,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
// Unwind the roll from the axes, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat(-roll, FLOATNAME(LVector3)(0.0, 0.0, 1.0),
rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0, 0.0, 1.0),
CS_yup_right);
x = x * rot_z;
@ -221,7 +221,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
// Unwind the heading, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat(-heading, FLOATNAME(LVector3)(0.0, 1.0, 0.0),
rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0, 1.0, 0.0),
CS_yup_right);
x = x * rot_y;
@ -237,7 +237,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0),
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0),
CS_yup_right);
x = x * rot_x;
@ -285,7 +285,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
// Unwind the heading, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat(-heading, FLOATNAME(LVector3)(0.0, 0.0, 1.0),
rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0, 0.0, 1.0),
CS_zup_right);
x = x * rot_z;
@ -301,7 +301,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0),
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0),
CS_zup_right);
x = x * rot_x;
@ -317,7 +317,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(-roll, FLOATNAME(LVector3)(0.0, 1.0, 0.0),
rot_y = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0, 1.0, 0.0),
CS_zup_right);
x = x * rot_y;
@ -361,7 +361,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(roll, FLOATNAME(LVector3)(0.0, 1.0, 0.0),
rot_y = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0, 1.0, 0.0),
CS_zup_right);
x = x * rot_y;
@ -378,7 +378,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
// Unwind the heading, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat(-heading, FLOATNAME(LVector3)(0.0, 0.0, 1.0),
rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0, 0.0, 1.0),
CS_zup_right);
x = x * rot_z;
@ -394,7 +394,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0),
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0),
CS_zup_right);
x = x * rot_x;
@ -441,7 +441,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(roll, FLOATNAME(LVector3)(0.0, 1.0, 0.0),
rot_y = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0, 1.0, 0.0),
CS_zup_right);
x = x * rot_y;
@ -458,7 +458,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
// Unwind the heading, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat(-heading, FLOATNAME(LVector3)(0.0, 0.0, 1.0),
rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0, 0.0, 1.0),
CS_zup_right);
x = x * rot_z;
@ -474,7 +474,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0),
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0),
CS_zup_right);
x = x * rot_x;

View File

@ -782,11 +782,13 @@ rotate_mat(FLOATTYPE angle, FLOATNAME(LVecBase3) axis,
// Normalize the axis.
/*
// hack check for prenormalization, only works for simple unit vecs,
// which is what we usually pass in anyway. screws up if you happen to
// 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) {
*/
FLOATTYPE length_sq = axis_0 * axis_0 + axis_1 * axis_1 + axis_2 * axis_2;
#ifdef _DEBUG
nassertr(length_sq != 0.0, ident_mat());
@ -796,7 +798,75 @@ rotate_mat(FLOATTYPE angle, FLOATNAME(LVecBase3) axis,
axis_0 *= recip_length;
axis_1 *= recip_length;
axis_2 *= recip_length;
// }
FLOATTYPE angle_rad=deg_2_rad(angle);
FLOATTYPE s,c;
csincos(angle_rad,&s,&c);
FLOATTYPE t = 1.0 - c;
FLOATTYPE t0,t1,t2,s0,s1,s2;
t0 = t * axis_0;
t1 = t * axis_1;
t2 = t * axis_2;
s0 = s * axis_0;
s1 = s * axis_1;
s2 = s * axis_2;
mat._m.m._00 = t0 * axis_0 + c;
mat._m.m._01 = t0 * axis_1 + s2;
mat._m.m._02 = t0 * axis_2 - s1;
mat._m.m._10 = t1 * axis_0 - s2;
mat._m.m._11 = t1 * axis_1 + c;
mat._m.m._12 = t1 * axis_2 + s0;
mat._m.m._20 = t2 * axis_0 + s1;
mat._m.m._21 = t2 * axis_1 - s0;
mat._m.m._22 = t2 * axis_2 + c;
/*
mat._m.m._00 = t * axis._v.v._0 * axis._v.v._0 + c;
mat._m.m._01 = t * axis._v.v._0 * axis._v.v._1 + s * axis._v.v._2;
mat._m.m._02 = t * axis._v.v._0 * axis._v.v._2 - s * axis._v.v._1;
mat._m.m._10 = t * axis._v.v._1 * axis._v.v._0 - s * axis._v.v._2;
mat._m.m._11 = t * axis._v.v._1 * axis._v.v._1 + c;
mat._m.m._12 = t * axis._v.v._1 * axis._v.v._2 + s * axis._v.v._0;
mat._m.m._20 = t * axis._v.v._2 * axis._v.v._0 + s * axis._v.v._1;
mat._m.m._21 = t * axis._v.v._2 * axis._v.v._1 - s * axis._v.v._0;
mat._m.m._22 = t * axis._v.v._2 * axis._v.v._2 + c;
*/
return mat;
}
////////////////////////////////////////////////////////////////////
// Function: LMatrix::rotate_mat_normaxis
// Access: Public, Static
// Description: Returns a matrix that rotates by the given angle in
// degrees counterclockwise about the indicated vector.
// Assumes axis has been normalized.
////////////////////////////////////////////////////////////////////
INLINE_LINMATH FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::
rotate_mat_normaxis(FLOATTYPE angle, FLOATNAME(LVecBase3) axis,
CoordinateSystem cs) {
if (cs == CS_default) {
cs = default_coordinate_system;
}
FLOATNAME(LMatrix3) mat;
if(IS_LEFT_HANDED_COORDSYSTEM(cs)) {
// In a left-handed coordinate system, counterclockwise is the
// other direction.
angle = -angle;
}
FLOATTYPE axis_0 = axis._v.v._0;
FLOATTYPE axis_1 = axis._v.v._1;
FLOATTYPE axis_2 = axis._v.v._2;
FLOATTYPE angle_rad=deg_2_rad(angle);
FLOATTYPE s,c;

View File

@ -119,6 +119,10 @@ PUBLISHED:
static INLINE_LINMATH FLOATNAME(LMatrix3) rotate_mat(FLOATTYPE angle,
FLOATNAME(LVecBase3) axis,
CoordinateSystem cs = CS_default);
static INLINE_LINMATH FLOATNAME(LMatrix3) rotate_mat_normaxis(FLOATTYPE angle,
FLOATNAME(LVecBase3) axis,
CoordinateSystem cs = CS_default);
static INLINE_LINMATH FLOATNAME(LMatrix3) scale_mat(const FLOATNAME(LVecBase3) &scale);
static INLINE_LINMATH FLOATNAME(LMatrix3) scale_mat(FLOATTYPE sx, FLOATTYPE sy, FLOATTYPE sz);

View File

@ -1137,13 +1137,16 @@ rotate_mat(FLOATTYPE angle, FLOATNAME(LVecBase3) axis,
FLOATTYPE axis_2 = axis._v.v._2;
// Normalize the axis.
/*
// hack check for prenormalization, only works for simple unit vecs,
// which is what we usually pass in anyway. screws up if you happen to
// 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) {
*/
FLOATTYPE length_sq = axis_0 * axis_0 + axis_1 * axis_1 + axis_2 * axis_2;
#ifdef _DEBUG
nassertr(length_sq != 0.0, ident_mat());
@ -1153,7 +1156,72 @@ rotate_mat(FLOATTYPE angle, FLOATNAME(LVecBase3) axis,
axis_0 *= recip_length;
axis_1 *= recip_length;
axis_2 *= recip_length;
// }
FLOATTYPE angle_rad=deg_2_rad(angle);
FLOATTYPE s,c;
csincos(angle_rad,&s,&c);
FLOATTYPE t = 1.0 - c;
FLOATTYPE t0,t1,t2,s0,s1,s2;
t0 = t * axis_0;
t1 = t * axis_1;
t2 = t * axis_2;
s0 = s * axis_0;
s1 = s * axis_1;
s2 = s * axis_2;
mat._m.m._00 = t0 * axis_0 + c;
mat._m.m._01 = t0 * axis_1 + s2;
mat._m.m._02 = t0 * axis_2 - s1;
mat._m.m._10 = t1 * axis_0 - s2;
mat._m.m._11 = t1 * axis_1 + c;
mat._m.m._12 = t1 * axis_2 + s0;
mat._m.m._20 = t2 * axis_0 + s1;
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._30 = 0.0;
mat._m.m._31 = 0.0;
mat._m.m._32 = 0.0;
mat._m.m._33 = 1.0;
return mat;
}
////////////////////////////////////////////////////////////////////
// Function: LMatrix::rotate_mat_normaxis
// Access: Public, Static
// Description: Returns a matrix that rotates by the given angle in
// degrees counterclockwise about the indicated vector.
// Assumes axis has been prenormalized.
////////////////////////////////////////////////////////////////////
INLINE_LINMATH FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::
rotate_mat_normaxis(FLOATTYPE angle, FLOATNAME(LVecBase3) axis,
CoordinateSystem cs) {
if (cs == CS_default) {
cs = default_coordinate_system;
}
FLOATNAME(LMatrix4) mat;
if(IS_LEFT_HANDED_COORDSYSTEM(cs)) {
// In a left-handed coordinate system, counterclockwise is the
// other direction.
angle = -angle;
}
FLOATTYPE axis_0 = axis._v.v._0;
FLOATTYPE axis_1 = axis._v.v._1;
FLOATTYPE axis_2 = axis._v.v._2;
FLOATTYPE angle_rad=deg_2_rad(angle);
FLOATTYPE s,c;

View File

@ -108,6 +108,9 @@ PUBLISHED:
INLINE_LINMATH static FLOATNAME(LMatrix4) rotate_mat(FLOATTYPE angle,
FLOATNAME(LVecBase3) axis,
CoordinateSystem cs = CS_default);
INLINE_LINMATH static FLOATNAME(LMatrix4) rotate_mat_normaxis(FLOATTYPE angle,
FLOATNAME(LVecBase3) axis,
CoordinateSystem cs = CS_default);
INLINE_LINMATH static FLOATNAME(LMatrix4) scale_mat(const FLOATNAME(LVecBase3) &scale);
INLINE_LINMATH static FLOATNAME(LMatrix4) scale_mat(FLOATTYPE sx, FLOATTYPE sy, FLOATTYPE sz);
INLINE_LINMATH static FLOATNAME(LMatrix4) scale_mat(FLOATTYPE scale);

View File

@ -323,7 +323,7 @@ apply(double x, double y, bool any_button) {
// rot_mat is the rotation matrix corresponding to our previous
// heading.
LMatrix3f rot_mat =
LMatrix3f::rotate_mat(_hpr[0], LVector3f::up(_cs), _cs);
LMatrix3f::rotate_mat_normaxis(_hpr[0], LVector3f::up(_cs), _cs);
// Take a step in the direction of our previous heading.
LVector3f step = distance * (LVector3f::forward(_cs) * rot_mat);

View File

@ -398,7 +398,7 @@ apply(double x, double y, int button) {
// screen.
_rotation *=
LMatrix4f::rotate_mat((x - y) * _rotscale,
LMatrix4f::rotate_mat_normaxis((x - y) * _rotscale,
LVector3f::forward(_cs), _cs);
} else if ((button == B2_MASK) || (button == (B1_MASK | B3_MASK))) {
@ -407,8 +407,8 @@ apply(double x, double y, int button) {
// support two-button mice.)
_rotation *=
LMatrix4f::rotate_mat(x * _rotscale, LVector3f::up(_cs), _cs) *
LMatrix4f::rotate_mat(y * _rotscale, LVector3f::right(_cs), _cs);
LMatrix4f::rotate_mat_normaxis(x * _rotscale, LVector3f::up(_cs), _cs) *
LMatrix4f::rotate_mat_normaxis(y * _rotscale, LVector3f::right(_cs), _cs);
} else if ((button == B3_MASK) || (button == (B1_MASK | B2_MASK))) {
// Button 3, or buttons 1 + 2: dolly in and out along the forward