<commit_before>/* -*- mode: C++; tab-width: 8; indent-tabs-mode: t; c-basic-offset: 8 -*- */
// vim:sts=8:sw=8:ts=8:noet:sr:cino=>s,f0,{0,g0,(0,\:0,t0,+0,=s

/*
 * Copyright (C) 2014 FFLAS-FFPACK group
 *
 * Written by Pascal Giorgi <pascal.giorgi@lirmm.fr>
 *
 *
 * ========LICENCE========
 * This file is part of the library FFLAS-FFPACK.
 *
 * FFLAS-FFPACK is free software: you can redistribute it and/or modify
 * it under the terms of the  GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 * ========LICENCE========
 *.
 */

#ifndef __FFLASFFPACK_fgemv_mp_INL
#define __FFLASFFPACK_fgemv_mp_INL

#include "fflas-ffpack/field/rns-integer-mod.h"

namespace FFLAS {

	// specialization of the fgemv function for the field RNSInteger<rns_double>
	inline FFPACK::rns_double::Element_ptr
	fgemv (const FFPACK::RNSInteger<FFPACK::rns_double>& F, const FFLAS_TRANSPOSE ta,
	       const size_t M, const size_t N,
	       const FFPACK::rns_double::Element alpha,
	       FFPACK::rns_double::ConstElement_ptr A, const size_t lda,
	       FFPACK::rns_double::ConstElement_ptr X, const size_t incX,
	        const FFPACK::rns_double::Element beta,
	       FFPACK::rns_double::Element_ptr Y, const size_t incY,
	       MMHelper<FFPACK::RNSInteger<FFPACK::rns_double>, MMHelperAlgo::Classic, ModeCategories::DefaultTag> & H)
	{
		if (M!=0 && N !=0){
			for (size_t i=0;i<F.size();i++)
				fgemv(F.rns()._field_rns[i], ta,
				      M, N,
				      alpha._ptr[i*alpha._stride],
				      A._ptr+i*A._stride, lda,
				      X._ptr+i*X._stride, incX,
				      beta._ptr[i*beta._stride],
				      Y._ptr+i*Y._stride, incY
				      );
		}
		return Y;
	}

	// specialization of the fgemv function for the field RNSIntegerMod<rns_double>
	inline FFPACK::rns_double::Element_ptr
	fgemv (const FFPACK::RNSIntegerMod<FFPACK::rns_double>& F, const FFLAS_TRANSPOSE ta,
	       const size_t M, const size_t N,
	       const FFPACK::rns_double::Element alpha,
	       FFPACK::rns_double::ConstElement_ptr A, const size_t lda,
	       FFPACK::rns_double::ConstElement_ptr X, const size_t incX,
	        const FFPACK::rns_double::Element beta,
	       FFPACK::rns_double::Element_ptr Y, const size_t incY,
	       MMHelper<FFPACK::RNSIntegerMod<FFPACK::rns_double>, MMHelperAlgo::Classic, ModeCategories::DefaultTag> & H)
	{
		//std::cout<<"HERE 1"<<std::endl;
		MMHelper<FFPACK::RNSInteger<FFPACK::rns_double>, MMHelperAlgo::Classic, ModeCategories::DefaultTag >  H2;
		//std::cout<<"HERE 2"<<std::endl;
		fgemv(F.delayed(),ta,M,N,alpha,A,lda,X,incX, beta,Y,incY,H2);
		//std::cout<<"HERE 3"<<std::endl;
		size_t Ydim = (ta == FflasNoTrans)?M:N;
		freduce (F, Ydim, Y, incY);
		return Y;
	}

	// BB hack. might not work.
	// specialization of the fgemv function for the field Givaro::ZRing<Givaro::Integer>
	inline Givaro::Integer* fgemv (const Givaro::ZRing<Givaro::Integer>& F,
				       const FFLAS_TRANSPOSE ta,
				       const size_t m, const size_t n,
				       const Givaro::Integer alpha,
				       Givaro::Integer* A, const size_t lda,
				       Givaro::Integer* X, const size_t ldx,
				       Givaro::Integer beta,
				       Givaro::Integer* Y, const size_t ldy,
				       MMHelper<Givaro::ZRing<Givaro::Integer>, MMHelperAlgo::Classic, ModeCategories::ConvertTo<ElementCategories::RNSElementTag> > & H)
	{
		MMHelper<Givaro::ZRing<Givaro::Integer>, MMHelperAlgo::Winograd, ModeCategories::ConvertTo<ElementCategories::RNSElementTag>, ParSeqHelper::Sequential> H2;
		fgemm(F,ta,FFLAS::FflasNoTrans,m,n,1,alpha,A,lda,X,ldx,beta,Y,ldy,H2);
		return Y;
	}

	// specialization of the fgemv function for the field Givaro::Modular<Givaro::Integer>
	inline Givaro::Integer* fgemv (const Givaro::Modular<Givaro::Integer>& F,
				       const FFLAS_TRANSPOSE ta,
				       const size_t m, const size_t n,
				       const Givaro::Integer alpha,
				       Givaro::Integer* A, const size_t lda,
				       Givaro::Integer* X, const size_t ldx,
				       Givaro::Integer beta,
				       Givaro::Integer* Y, const size_t ldy,
				       MMHelper<Givaro::Modular<Givaro::Integer>, MMHelperAlgo::Classic, ModeCategories::ConvertTo<ElementCategories::RNSElementTag> > & H)
	{
		MMHelper<Givaro::Modular<Givaro::Integer>, MMHelperAlgo::Winograd, ModeCategories::ConvertTo<ElementCategories::RNSElementTag>, ParSeqHelper::Sequential> H2;
		fgemm(F,ta,FFLAS::FflasNoTrans,m,n,1,alpha,A,lda,X,ldx,beta,Y,ldy,H2);
		return Y;
	}

} // end namespace FFLAS

#endif

<commit_msg>fix bug in fgemv over multiprecision<commit_after>/* -*- mode: C++; tab-width: 8; indent-tabs-mode: t; c-basic-offset: 8 -*- */
// vim:sts=8:sw=8:ts=8:noet:sr:cino=>s,f0,{0,g0,(0,\:0,t0,+0,=s

/*
 * Copyright (C) 2014 FFLAS-FFPACK group
 *
 * Written by Pascal Giorgi <pascal.giorgi@lirmm.fr>
 *
 *
 * ========LICENCE========
 * This file is part of the library FFLAS-FFPACK.
 *
 * FFLAS-FFPACK is free software: you can redistribute it and/or modify
 * it under the terms of the  GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 * ========LICENCE========
 *.
 */

#ifndef __FFLASFFPACK_fgemv_mp_INL
#define __FFLASFFPACK_fgemv_mp_INL

#include "fflas-ffpack/field/rns-integer-mod.h"

namespace FFLAS {

	// specialization of the fgemv function for the field RNSInteger<rns_double>
	inline FFPACK::rns_double::Element_ptr
	fgemv (const FFPACK::RNSInteger<FFPACK::rns_double>& F, const FFLAS_TRANSPOSE ta,
	       const size_t M, const size_t N,
	       const FFPACK::rns_double::Element alpha,
	       FFPACK::rns_double::ConstElement_ptr A, const size_t lda,
	       FFPACK::rns_double::ConstElement_ptr X, const size_t incX,
	        const FFPACK::rns_double::Element beta,
	       FFPACK::rns_double::Element_ptr Y, const size_t incY,
	       MMHelper<FFPACK::RNSInteger<FFPACK::rns_double>, MMHelperAlgo::Classic, ModeCategories::DefaultTag> & H)
	{
		if (M!=0 && N !=0){
			for (size_t i=0;i<F.size();i++)
				fgemv(F.rns()._field_rns[i], ta,
				      M, N,
				      alpha._ptr[i*alpha._stride],
				      A._ptr+i*A._stride, lda,
				      X._ptr+i*X._stride, incX,
				      beta._ptr[i*beta._stride],
				      Y._ptr+i*Y._stride, incY
				      );
		}
		return Y;
	}

	// specialization of the fgemv function for the field RNSIntegerMod<rns_double>
	inline FFPACK::rns_double::Element_ptr
	fgemv (const FFPACK::RNSIntegerMod<FFPACK::rns_double>& F, const FFLAS_TRANSPOSE ta,
	       const size_t M, const size_t N,
	       const FFPACK::rns_double::Element alpha,
	       FFPACK::rns_double::ConstElement_ptr A, const size_t lda,
	       FFPACK::rns_double::ConstElement_ptr X, const size_t incX,
	        const FFPACK::rns_double::Element beta,
	       FFPACK::rns_double::Element_ptr Y, const size_t incY,
	       MMHelper<FFPACK::RNSIntegerMod<FFPACK::rns_double>, MMHelperAlgo::Classic, ModeCategories::DefaultTag> & H)
	{
		//std::cout<<"HERE 1"<<std::endl;
		MMHelper<FFPACK::RNSInteger<FFPACK::rns_double>, MMHelperAlgo::Classic, ModeCategories::DefaultTag >  H2;
		//std::cout<<"HERE 2"<<std::endl;
		fgemv(F.delayed(),ta,M,N,alpha,A,lda,X,incX, beta,Y,incY,H2);
		//std::cout<<"HERE 3"<<std::endl;
		size_t Ydim = (ta == FflasNoTrans)?M:N;
		freduce (F, Ydim, Y, incY);
		return Y;
	}

	// BB hack. might not work.
	// specialization of the fgemv function for the field Givaro::ZRing<Givaro::Integer>
	inline Givaro::Integer* fgemv (const Givaro::ZRing<Givaro::Integer>& F,
				       const FFLAS_TRANSPOSE ta,
				       const size_t m, const size_t n,
				       const Givaro::Integer alpha,
				       Givaro::Integer* A, const size_t lda,
				       Givaro::Integer* X, const size_t ldx,
				       Givaro::Integer beta,
				       Givaro::Integer* Y, const size_t ldy,
				       MMHelper<Givaro::ZRing<Givaro::Integer>, MMHelperAlgo::Classic, ModeCategories::ConvertTo<ElementCategories::RNSElementTag> > & H)
	{
		MMHelper<Givaro::ZRing<Givaro::Integer>, MMHelperAlgo::Winograd, ModeCategories::ConvertTo<ElementCategories::RNSElementTag>, ParSeqHelper::Sequential> H2;
		fgemm(F,ta,FFLAS::FflasNoTrans, (ta==FFLAS::FflasNoTrans)?m:n, 1,(ta==FFLAS::FflasNoTrans)?n:m, alpha,A,lda,X,ldx,beta,Y,ldy,H2);
		return Y;
	}

	// specialization of the fgemv function for the field Givaro::Modular<Givaro::Integer>
	inline Givaro::Integer* fgemv (const Givaro::Modular<Givaro::Integer>& F,
				       const FFLAS_TRANSPOSE ta,
				       const size_t m, const size_t n,
				       const Givaro::Integer alpha,
				       Givaro::Integer* A, const size_t lda,
				       Givaro::Integer* X, const size_t ldx,
				       Givaro::Integer beta,
				       Givaro::Integer* Y, const size_t ldy,
				       MMHelper<Givaro::Modular<Givaro::Integer>, MMHelperAlgo::Classic, ModeCategories::ConvertTo<ElementCategories::RNSElementTag> > & H)
	{
		MMHelper<Givaro::Modular<Givaro::Integer>, MMHelperAlgo::Winograd, ModeCategories::ConvertTo<ElementCategories::RNSElementTag>, ParSeqHelper::Sequential> H2;
		fgemm(F,ta,FFLAS::FflasNoTrans,(ta==FFLAS::FflasNoTrans)?m:n,1,(ta==FFLAS::FflasNoTrans)?n:m,alpha,A,lda,X,ldx,beta,Y,ldy,H2);
		return Y;
	}

} // end namespace FFLAS

#endif

<|endoftext|>


<commit_before>/*
 * Copyright (C) 2014 FFLAS-FFACK group
 *
 * Written by Clement Pernet <Clement.Pernet@imag.fr>
 * Brice Boyer (briceboyer) <boyer.brice@gmail.com>
 *
 *
 * ========LICENCE========
 * This file is part of the library FFLAS-FFPACK.
 *
 * FFLAS-FFPACK is free software: you can redistribute it and/or modify
 * it under the terms of the  GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 * ========LICENCE========
 *.
 */

#ifndef __FFLASFFPACK_ffpack_fgetrs_INL
#define __FFLASFFPACK_ffpack_fgetrs_INL

namespace FFPACK {

    template <class Field>
    void
    fgetrs (const Field& F,
            const FFLAS::FFLAS_SIDE Side,
            const size_t M, const size_t N, const size_t R,
            typename Field::Element_ptr A, const size_t lda,
            const size_t *P, const size_t *Q,
            typename Field::Element_ptr B, const size_t ldb,
            int * info)
    {

        *info =0;
        if (Side == FFLAS::FflasLeft) { // Left looking solve A X = B

//             // Code for when A is LU decomposed with LUdivine
//
//             solveLB2 (F, FFLAS::FflasLeft, M, N, R, A, lda, Q, B, ldb);

//             applyP (F, FFLAS::FflasLeft, FFLAS::FflasNoTrans,
//                     N, 0,(int) R, B, ldb, Q);

//             bool consistent = true;
//             for (size_t i = R; i < M; ++i)
//                 for (size_t j = 0; j < N; ++j)
//                     if (!F.isZero (*(B + i*ldb + j)))
//                         consistent = false;
//             if (!consistent) {
//                 *info = 1;
//             }
//             // The last rows of B are now supposed to be 0
// #if 0
//             for (size_t i = R; i < M; ++i)
//                 for (size_t j = 0; j < N; ++j)
//                     *(B + i*ldb + j) = F.zero;
// #endif

                // B <- P^T B
            applyP (F, FFLAS::FflasLeft, FFLAS::FflasNoTrans, N, 0, M, B, ldb, P);
                // B <- L^-1 B
            ftrsm (F, FFLAS::FflasLeft, FFLAS::FflasLower, FFLAS::FflasNoTrans, FFLAS::FflasUnit,
                   R, N, F.one, A, lda , B, ldb);

                // Verifying that the system is consistent @todo: disable this check optionnally
            fgemm (F, FFLAS::FflasNoTrans, FFLAS::FflasNoTrans, M-R, N, R, F.mOne, A+R*lda, lda, B, ldb, F.One, B+R*ldb, ldb);
            if (! fiszero (F, M-R, N, B+R*ldb, ldb)) *info = 1;

                // B <- U^-1 B
            ftrsm (F, FFLAS::FflasLeft, FFLAS::FflasUpper, FFLAS::FflasNoTrans, FFLAS::FflasNonUnit,
                   R, N, F.one, A, lda , B, ldb);
                // B <- P^T B
            applyP (F, FFLAS::FflasLeft, FFLAS::FflasTrans, N, 0, M, B, ldb, Q);

        }
        else { // Right Looking X A = B

                // B <- B Q^T
            applyP (F, FFLAS::FflasRight, FFLAS::FflasTrans, M, 0, N, B, ldb, Q);
                // B <- B U^-1
            ftrsm (F, FFLAS::FflasRight, FFLAS::FflasUpper, FFLAS::FflasNoTrans, FFLAS::FflasNonUnit,
                   M, R, F.one, A, lda , B, ldb);
                //  Verifying that the system is consistent @todo: disable this check optionnally
            fgemm (F, FFLAS::FflasNoTrans, FFLAS::FflasNoTrans, M, N-R, R, F.mOne, B, ldb, A+R, lda, F.one, B+R, ldb);
            if (! fiszero (F, M, N-R, B+R, ldb)) *info = 1;

                // B <- B L^-1
            ftrsm (F, FFLAS::FflasRight, FFLAS::FflasLower, FFLAS::FflasNoTrans, FFLAS::FflasUnit,
                   M, R, F.one, A, lda , B, ldb);

                // B <- B P^T
            applyP (F, FFLAS::FflasRight, FFLAS::FflasNoTrans, M, 0, M, B, ldb, P);
        }
    }

    template <class Field>
    typename Field::Element_ptr
    fgetrs (const Field& F,
            const FFLAS::FFLAS_SIDE Side,
            const size_t M, const size_t N, const size_t NRHS, const size_t R,
            typename Field::Element_ptr A, const size_t lda,
            const size_t *P, const size_t *Q,
            typename Field::Element_ptr X, const size_t ldx,
            typename Field::ConstElement_ptr B, const size_t ldb,
            int * info)
    {
        *info =0;
        typename Field::Element_ptr W;
        size_t ldw;

        if (Side == FFLAS::FflasLeft) { // Left looking solve A X = B

            // Initializing X to 0 (to be optimized)
            FFLAS::fzero(F,N,NRHS,X,ldx);
            // for (size_t i = 0; i <N; ++i)
            // for (size_t j=0; j< NRHS; ++j)
            // F.assign (*(X+i*ldx+j), F.zero);

            if (M > N){ // Cannot copy B into X
                W = FFLAS::fflas_new (F, M, NRHS);
                ldw = NRHS;
                FFLAS::fassign(F,M,NRHS,B,ldb,W,ldw);

                solveLB2 (F, FFLAS::FflasLeft, M, NRHS, R, A, lda, Q, W, ldw);

                applyP (F, FFLAS::FflasLeft, FFLAS::FflasNoTrans,
                        NRHS, 0,(int) R, W, ldw, Q);

                bool consistent = true;
                for (size_t i = R; i < M; ++i)
                    for (size_t j = 0; j < NRHS; ++j)
                        if (!F.isZero (*(W + i*ldw + j)))
                            consistent = false;
                if (!consistent) {
                    *info = 1;
                    FFLAS::fflas_delete (W);
                    return X;
                }
                // Here the last rows of W are supposed to be 0

                ftrsm (F, FFLAS::FflasLeft, FFLAS::FflasUpper, FFLAS::FflasNoTrans, FFLAS::FflasNonUnit,
                       R, NRHS, F.one, A, lda , W, ldw);

                FFLAS::fassign(F,R,NRHS,W,ldw,X,ldx);

                FFLAS::fflas_delete (W);
                applyP (F, FFLAS::FflasLeft, FFLAS::FflasTrans,
                        NRHS, 0,(int) R, X, ldx, P);

            }
            else { // Copy B to X directly

                FFLAS::fassign(F,M,NRHS,B,ldb,X,ldx);

                solveLB2 (F, FFLAS::FflasLeft, M, NRHS, R, A, lda, Q, X, ldx);

                applyP (F, FFLAS::FflasLeft, FFLAS::FflasNoTrans,
                        NRHS, 0,(int) R, X, ldx, Q);

                bool consistent = true;
                for (size_t i = R; i < M; ++i)
                    for (size_t j = 0; j < NRHS; ++j)
                        if (!F.isZero (*(X + i*ldx + j)))
                            consistent = false;
                if (!consistent) {
                    *info = 1;
                    return X;
                }
                // Here the last rows of W are supposed to be 0

                ftrsm (F, FFLAS::FflasLeft, FFLAS::FflasUpper, FFLAS::FflasNoTrans, FFLAS::FflasNonUnit,
                       R, NRHS, F.one, A, lda , X, ldx);

                applyP (F, FFLAS::FflasLeft, FFLAS::FflasTrans,
                        NRHS, 0,(int) R, X, ldx, P);
            }
            return X;

        }
        else { // Right Looking X A = B

            FFLAS::fzero(F,NRHS,M,X,ldx);
            // for (size_t i = 0; i <NRHS; ++i)
            // for (size_t j=0; j< M; ++j)
            // F.assign (*(X+i*ldx+j), F.zero);

            if (M < N) {
                W = FFLAS::fflas_new (F, NRHS, N);
                ldw = N;
                FFLAS::fassign (F,NRHS, N, B, ldb, W, ldw);

                applyP (F, FFLAS::FflasRight, FFLAS::FflasTrans,
                        NRHS, 0,(int) R, W, ldw, P);

                ftrsm (F, FFLAS::FflasRight, FFLAS::FflasUpper, FFLAS::FflasNoTrans, FFLAS::FflasNonUnit,
                       NRHS, R, F.one, A, lda , W, ldw);

                fgemm (F, FFLAS::FflasNoTrans, FFLAS::FflasNoTrans, NRHS, N-R, R, F.one,
                       W, ldw, A+R, lda, F.mOne, W+R, ldw);

                bool consistent = true;
                for (size_t i = 0; i < NRHS; ++i)
                    for (size_t j = R; j < N; ++j)
                        if (!F.isZero (*(W + i*ldw + j)))
                            consistent = false;
                if (!consistent) {
                    *info = 1;
                    FFLAS::fflas_delete (W);
                    return X;
                }
                // The last N-R cols of W are now supposed to be 0
                FFLAS::fassign (F, NRHS,R,  W , ldb, X ,ldx);
                FFLAS::fflas_delete (W);
                applyP (F, FFLAS::FflasRight, FFLAS::FflasNoTrans,
                        NRHS, 0,(int) R, X, ldx, Q);

                solveLB2 (F, FFLAS::FflasRight, NRHS, M, R, A, lda, Q, X, ldx);

            }
            else { // M >=N
                FFLAS::fassign(F,NRHS,N,B,ldb,X,ldx);

                applyP (F, FFLAS::FflasRight, FFLAS::FflasTrans,
                        NRHS, 0,(int) R, X, ldx, P);

                ftrsm (F, FFLAS::FflasRight, FFLAS::FflasUpper, FFLAS::FflasNoTrans, FFLAS::FflasNonUnit,
                       NRHS, R, F.one, A, lda , X, ldx);

                fgemm (F, FFLAS::FflasNoTrans, FFLAS::FflasNoTrans, NRHS, N-R, R, F.one,
                       X, ldx, A+R, lda, F.mOne, X+R, ldx);

                bool consistent = true;
                for (size_t i = 0; i < NRHS; ++i)
                    for (size_t j = R; j < N; ++j)
                        if (!F.isZero (*(X + i*ldx + j)))
                            consistent = false;
                if (!consistent) {
                    *info = 1;
                    return X;
                }
                // The last N-R cols of W are now supposed to be 0

                applyP (F, FFLAS::FflasRight, FFLAS::FflasNoTrans,
                        NRHS, 0,(int) R, X, ldx, Q);

                solveLB2 (F, FFLAS::FflasRight, NRHS, M, R, A, lda, Q, X, ldx);

            }
            return X;
        }
    }

} // FFPACK

#endif // __FFLASFFPACK_ffpack_fgetrs_INL
/* -*- mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
// vim:sts=4:sw=4:ts=4:et:sr:cino=>s,f0,{0,g0,(0,\:0,t0,+0,=s
<commit_msg>fix fgetrs accordingly<commit_after>/*
 * Copyright (C) 2014 FFLAS-FFACK group
 *
 * Written by Clement Pernet <Clement.Pernet@imag.fr>
 * Brice Boyer (briceboyer) <boyer.brice@gmail.com>
 *
 *
 * ========LICENCE========
 * This file is part of the library FFLAS-FFPACK.
 *
 * FFLAS-FFPACK is free software: you can redistribute it and/or modify
 * it under the terms of the  GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 * ========LICENCE========
 *.
 */

#ifndef __FFLASFFPACK_ffpack_fgetrs_INL
#define __FFLASFFPACK_ffpack_fgetrs_INL

namespace FFPACK {

    template <class Field>
    void
    fgetrs (const Field& F,
            const FFLAS::FFLAS_SIDE Side,
            const size_t M, const size_t N, const size_t R,
            typename Field::Element_ptr A, const size_t lda,
            const size_t *P, const size_t *Q,
            typename Field::Element_ptr B, const size_t ldb,
            int * info)
    {

        *info =0;
        if (Side == FFLAS::FflasLeft) { // Left looking solve A X = B

                // B <- P^T B
            applyP (F, FFLAS::FflasLeft, FFLAS::FflasNoTrans, N, 0, M, B, ldb, P);
                // B <- L^-1 B
            ftrsm (F, FFLAS::FflasLeft, FFLAS::FflasLower, FFLAS::FflasNoTrans, FFLAS::FflasUnit,
                   R, N, F.one, A, lda , B, ldb);

                // Verifying that the system is consistent @todo: disable this check optionnally
            fgemm (F, FFLAS::FflasNoTrans, FFLAS::FflasNoTrans, M-R, N, R, F.mOne, A+R*lda, lda, B, ldb, F.One, B+R*ldb, ldb);
            if (! fiszero (F, M-R, N, B+R*ldb, ldb)) *info = 1;

                // B <- U^-1 B
            ftrsm (F, FFLAS::FflasLeft, FFLAS::FflasUpper, FFLAS::FflasNoTrans, FFLAS::FflasNonUnit,
                   R, N, F.one, A, lda , B, ldb);
                // B <- P^T B
            applyP (F, FFLAS::FflasLeft, FFLAS::FflasTrans, N, 0, M, B, ldb, Q);

        }
        else { // Right Looking X A = B

                // B <- B Q^T
            applyP (F, FFLAS::FflasRight, FFLAS::FflasTrans, M, 0, N, B, ldb, Q);
                // B <- B U^-1
            ftrsm (F, FFLAS::FflasRight, FFLAS::FflasUpper, FFLAS::FflasNoTrans, FFLAS::FflasNonUnit,
                   M, R, F.one, A, lda , B, ldb);
                //  Verifying that the system is consistent @todo: disable this check optionnally
            fgemm (F, FFLAS::FflasNoTrans, FFLAS::FflasNoTrans, M, N-R, R, F.mOne, B, ldb, A+R, lda, F.one, B+R, ldb);
            if (! fiszero (F, M, N-R, B+R, ldb)) *info = 1;

                // B <- B L^-1
            ftrsm (F, FFLAS::FflasRight, FFLAS::FflasLower, FFLAS::FflasNoTrans, FFLAS::FflasUnit,
                   M, R, F.one, A, lda , B, ldb);

                // B <- B P^T
            applyP (F, FFLAS::FflasRight, FFLAS::FflasNoTrans, M, 0, M, B, ldb, P);
        }
    }

    template <class Field>
    typename Field::Element_ptr
    fgetrs (const Field& F,
            const FFLAS::FFLAS_SIDE Side,
            const size_t M, const size_t N, const size_t NRHS, const size_t R,
            typename Field::Element_ptr A, const size_t lda,
            const size_t *P, const size_t *Q,
            typename Field::Element_ptr X, const size_t ldx,
            typename Field::ConstElement_ptr B, const size_t ldb,
            int * info)
    {
        *info =0;
        typename Field::Element_ptr W;
        size_t ldw;

        if (Side == FFLAS::FflasLeft) { // Left looking solve A X = B

            if (M > N){ // Cannot copy B into X
                W = FFLAS::fflas_new (F, M, NRHS);
                ldw = NRHS;

                FFLAS::fassign(F,M,NRHS,B,ldb,W,ldw);
                    // B <- P^T B
                applyP (F, FFLAS::FflasLeft, FFLAS::FflasNoTrans, NRHS, 0, M, W, ldw, P);
                    // B <- L^-1 B
                ftrsm (F, FFLAS::FflasLeft, FFLAS::FflasLower, FFLAS::FflasNoTrans, FFLAS::FflasUnit,
                       R, NRHS, F.one, A, lda , W, ldw);

                    // Verifying that the system is consistent @todo: disable this check optionnally
                fgemm (F, FFLAS::FflasNoTrans, FFLAS::FflasNoTrans, M-R, NRHS, R, F.mOne, A+R*lda, lda, W, ldw, F.one, W+R*ldw, ldw);
                if (! fiszero (F, M-R, NRHS, W+R*ldw, ldw)) *info = 1;

                    // B <- U^-1 B
                ftrsm (F, FFLAS::FflasLeft, FFLAS::FflasUpper, FFLAS::FflasNoTrans, FFLAS::FflasNonUnit,
                       R, NRHS, F.one, A, lda , W, ldw);

                FFLAS::fassign (F,R,NRHS,W,ldw,X,ldx);
                FFLAS::fflas_delete (W);

                FFLAS::fzero (F, N-R, NRHS, X+R, ldx);

                    // B <- Q^T B
                applyP (F, FFLAS::FflasLeft, FFLAS::FflasTrans, NRHS, 0, N, X, ldx, Q);

            }
            else { // Copy B to X directly

                FFLAS::fassign (F,M,NRHS,B,ldb,X,ldx);

                    // B <- P^T B
                applyP (F, FFLAS::FflasLeft, FFLAS::FflasNoTrans, NRHS, 0, M, X, ldx, P);
                    // B <- L^-1 B
                ftrsm (F, FFLAS::FflasLeft, FFLAS::FflasLower, FFLAS::FflasNoTrans, FFLAS::FflasUnit,
                       R, NRHS, F.one, A, lda , X, ldx);

                    // Verifying that the system is consistent @todo: disable this check optionnally
                fgemm (F, FFLAS::FflasNoTrans, FFLAS::FflasNoTrans, M-R, NRHS, R, F.mOne, A+R*lda, lda, X, ldx, F.one, X+R*ldx, ldx);
                if (! fiszero (F, M-R, NRHS, X+R*ldx, ldx)) *info = 1;

                    // B <- U^-1 B
                ftrsm (F, FFLAS::FflasLeft, FFLAS::FflasUpper, FFLAS::FflasNoTrans, FFLAS::FflasNonUnit,
                       R, NRHS, F.one, A, lda , X, ldx);

                FFLAS::fzero (F, N-M, NRHS, X+M, ldx);

                    // B <- Q^T B
                applyP (F, FFLAS::FflasLeft, FFLAS::FflasTrans, NRHS, 0, N, X, ldx, Q);
            }
            return X;

        }
        else { // Right Looking X A = B

            if (M < N) {
                W = FFLAS::fflas_new (F, NRHS, N);
                ldw = N;
                fassign (F,NRHS, N, B, ldb, W, ldw);

                    // B <- B Q^T
                applyP (F, FFLAS::FflasRight, FFLAS::FflasTrans, NRHS, 0, N, W, ldw, Q);

                    // B <- B U^-1
                ftrsm (F, FFLAS::FflasRight, FFLAS::FflasUpper, FFLAS::FflasNoTrans, FFLAS::FflasNonUnit,
                       NRHS, R, F.one, A, lda , W, ldw);

                    // Verifying that the system is consistent @todo: disable this check optionnally
                fgemm (F, FFLAS::FflasNoTrans, FFLAS::FflasNoTrans, NRHS, N-R, R, F.mOne,
                       W, ldw, A+R, lda, F.one, W+R, ldw);

                if (! fiszero (F, NRHS, N-R, W+R, ldw)) *info = 1;

                    // B <- B L^-1
                ftrsm (F, FFLAS::FflasRight, FFLAS::FflasLower, FFLAS::FflasNoTrans, FFLAS::FflasUnit,
                       NRHS, R, F.one, A, lda , W, ldw);

                    // X <- B
                fassign (F, NRHS, R, W, ldw, X, ldx);
                fflas_delete (W);

                fzero (F, NRHS, M-R, X+R, ldx);

                    // X <- X P^T
                applyP (F, FFLAS::FflasRight, FFLAS::FflasNoTrans, NRHS, 0, M, X, ldx, P);

            } else { // M >=N
                FFLAS::fassign (F, NRHS, N, B, ldb, X, ldx);

                    // B <- B Q^T
                applyP (F, FFLAS::FflasRight, FFLAS::FflasTrans, NRHS, 0, N, X, ldx, Q);

                    // B <- B U^-1
                ftrsm (F, FFLAS::FflasRight, FFLAS::FflasUpper, FFLAS::FflasNoTrans, FFLAS::FflasNonUnit,
                       NRHS, R, F.one, A, lda , X, ldx);

                    // Verifying that the system is consistent @todo: disable this check optionnally
                fgemm (F, FFLAS::FflasNoTrans, FFLAS::FflasNoTrans, NRHS, N-R, R, F.mOne,
                       X, ldx, A+R, lda, F.one, X+R, ldx);

                if (! fiszero (F, NRHS, N-R, X+R, ldx)) *info = 1;

                // B <- B L^-1
                ftrsm (F, FFLAS::FflasRight, FFLAS::FflasLower, FFLAS::FflasNoTrans, FFLAS::FflasUnit,
                       NRHS, R, F.one, A, lda , X, ldx);

                FFLAS::fzero (F, NRHS, M-N, X+N, ldx);

                    // X <- X P^T
                applyP (F, FFLAS::FflasRight, FFLAS::FflasNoTrans, NRHS, 0, M, X, ldx, P);
            }
            return X;
        }
    }

} // FFPACK

#endif // __FFLASFFPACK_ffpack_fgetrs_INL
/* -*- mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
// vim:sts=4:sw=4:ts=4:et:sr:cino=>s,f0,{0,g0,(0,\:0,t0,+0,=s
<|endoftext|>


<commit_before>#include <Windows.h>
#include <gdiplus.h>
#pragma comment(lib, "gdiplus.lib")
#include <iostream>
#include <string>
#include <unordered_map>
#include <Wtsapi32.h>

#include "3RVX.h"
#include "DisplayManager.h"
#include "HotkeyInfo.h"
#include "HotkeyManager.h"
#include "Logger.h"
#include "MediaKeys.h"
#include "OSD\EjectOSD.h"
#include "OSD\VolumeOSD.h"
#include "Settings.h"
#include "SkinManager.h"

HANDLE mutex;
HINSTANCE hInst;
ULONG_PTR gdiplusToken;
HWND mainWnd;

VolumeOSD *vOSD;
EjectOSD *eOSD;

HotkeyManager *hkManager;
std::unordered_map<int, HotkeyInfo> hotkeys;

void init();
HWND CreateMainWnd(HINSTANCE hInstance);
void ProcessHotkeys(HotkeyInfo &hki);
LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam);

int WINAPI wWinMain(_In_ HINSTANCE hInstance, _In_opt_ HINSTANCE hPrevInstance,
    _In_ LPWSTR lpCmdLine, _In_ int nShowCmd) {

    hInst = hInstance;

    Logger::Start();

    QCLOG(L"  _____ ______     ____  _______ ");
    QCLOG(L" |___ /|  _ \\ \\   / /\\ \\/ /___ / ");
    QCLOG(L"   |_ \\| |_) \\ \\ / /  \\  /  |_ \\ ");
    QCLOG(L"  ___) |  _ < \\ V /   /  \\ ___) |");
    QCLOG(L" |____/|_| \\_\\ \\_/   /_/\\_\\____/ ");
    QCLOG(L"");

    QCLOG(L"Starting up...");

    mutex = CreateMutex(NULL, FALSE, L"Local\\3RVX");
    if (GetLastError() == ERROR_ALREADY_EXISTS) {
        if (mutex) {
            ReleaseMutex(mutex);
        }

        HWND masterWnd = FindWindow(CLASS_3RVX, CLASS_3RVX);
        CLOG(L"A previous instance of the program is already running.\n"
            L"Requesting Settings dialog from window: %d", (int) masterWnd);
        SendMessage(masterWnd, WM_3RVX_CONTROL, MSG_SETTINGS, NULL);

#if defined(ENABLE_3RVX_LOG) && (defined(ENABLE_3RVX_LOGTOFILE) == FALSE)
        CLOG(L"Press [enter] to terminate");
        std::cin.get();
#endif

        return EXIT_SUCCESS;
    }

    CLOG(L"App directory: %s", Settings::AppDir().c_str());

    using namespace Gdiplus;
    GdiplusStartupInput gdiplusStartupInput;
    GdiplusStartup(&gdiplusToken, &gdiplusStartupInput, NULL);

    mainWnd = CreateMainWnd(hInstance);
    if (mainWnd == NULL) {
        CLOG(L"Could not create main window");
        return EXIT_FAILURE;
    }

    HRESULT hr = CoInitializeEx(NULL,
        COINIT_APARTMENTTHREADED | COINIT_DISABLE_OLE1DDE);
    if (hr != S_OK) {
        CLOG(L"Failed to initialize the COM library.");
        return EXIT_FAILURE;
    }

    /* Tell the program to initialize */
    PostMessage(mainWnd, WM_3RVX_CONTROL, MSG_LOAD, NULL);

    /* Start the event loop */
    MSG msg;
    while (GetMessage(&msg, NULL, 0, 0)) {
        TranslateMessage(&msg);
        DispatchMessage(&msg);
    }

    GdiplusShutdown(gdiplusToken);
    CoUninitialize();

    Logger::Stop();

    return (int) msg.wParam;
}

void init() {
    CLOG(L"Initializing...");

    delete vOSD;
    delete eOSD;

    Settings *settings = Settings::Instance();
    settings->Load();

    SkinManager::Instance()->LoadSkin(settings->SkinXML());

    /* TODO: Detect monitor changes, update this map, and reload/reorg OSDs */
    DisplayManager::UpdateMonitorMap();

    /* OSDs */
    eOSD = new EjectOSD();
    vOSD = new VolumeOSD();

    /* Hotkey setup */
    if (hkManager != NULL) {
        hkManager->Shutdown();
    }
    hkManager = HotkeyManager::Instance(mainWnd);

    hotkeys = Settings::Instance()->Hotkeys();
    for (auto it = hotkeys.begin(); it != hotkeys.end(); ++it) {
        int combination = it->first;
        hkManager->Register(combination);
    }

    WTSRegisterSessionNotification(mainWnd, NOTIFY_FOR_THIS_SESSION);
}

HWND CreateMainWnd(HINSTANCE hInstance) {
    WNDCLASSEX wcex;

    wcex.cbSize = sizeof(WNDCLASSEX);
    wcex.style = NULL;
    wcex.lpfnWndProc = WndProc;
    wcex.cbClsExtra = NULL;
    wcex.cbWndExtra = NULL;
    wcex.hInstance = hInstance;
    wcex.hIcon = LoadIcon(NULL, IDI_APPLICATION);
    wcex.hCursor = LoadCursor(NULL, IDC_ARROW);
    wcex.hbrBackground = (HBRUSH)(COLOR_WINDOW + 1);
    wcex.lpszMenuName = NULL;
    wcex.lpszClassName = CLASS_3RVX;
    wcex.hIconSm = LoadIcon(NULL, IDI_APPLICATION);

    if (!RegisterClassEx(&wcex)) {
        return NULL;
    }

    HWND hWnd = CreateWindowEx(
        NULL,
        CLASS_3RVX, CLASS_3RVX,
        NULL, NULL, NULL, //your boat, gently down the stream
        NULL, NULL, NULL, NULL, hInstance, NULL);

    return hWnd;
}

void ProcessHotkeys(HotkeyInfo &hki) {
    switch (hki.action) {
    case HotkeyInfo::IncreaseVolume:
    case HotkeyInfo::DecreaseVolume:
    case HotkeyInfo::Mute:
    case HotkeyInfo::VolumeSlider:
        if (vOSD) {
            vOSD->ProcessHotkeys(hki);
        }
        break;

    case HotkeyInfo::EjectDrive:
    case HotkeyInfo::EjectLatestDrive:
        if (eOSD) {
            eOSD->ProcessHotkeys(hki);
        }
        break;

    case HotkeyInfo::MediaKey:
        MediaKeys::ProcessHotkeys(hki);
        break;

    case HotkeyInfo::Settings:
        ShellExecute(NULL, L"open",
            Settings::SettingsApp().c_str(), NULL, NULL, SW_SHOWNORMAL);
        break;

    case HotkeyInfo::Exit:
        SendMessage(mainWnd, WM_CLOSE, NULL, NULL);
        break;
    }
}

LRESULT CALLBACK WndProc(
    HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam) {

    switch (message) {
    case WM_HOTKEY: {
        CLOG(L"Hotkey: %d", (int) wParam);
        HotkeyInfo hki = hotkeys[(int) wParam];
        ProcessHotkeys(hki);
        break;
    }

    case WM_WTSSESSION_CHANGE: {
        CLOG(L"Detected session change");
        break;
    }

    case WM_CLOSE: {
        CLOG(L"Shutting down");
        HotkeyManager::Instance()->Shutdown();
        vOSD->HideIcon();
        DestroyWindow(mainWnd);
        break;
    }

    case WM_DESTROY: {
        PostQuitMessage(0);
        break;
    }
    }

    if (message == WM_3RVX_CONTROL) {
        switch (wParam) {
        case MSG_LOAD:
            init();
            break;

        case MSG_SETTINGS:
            Settings::LaunchSettingsApp();
            break;

        case MSG_HIDEOSD:
            int except = (OSDType) lParam;
            switch (except) {
            case Volume:
                if (eOSD) {
                    eOSD->Hide();
                }
                break;

            case Eject:
                if (vOSD) {
                    vOSD->Hide();
                }
                break;
            }

            break;
        }
    }

    return DefWindowProc(hWnd, message, wParam, lParam);
}<commit_msg>Map SetVolume hotkey to the volume OSD<commit_after>#include <Windows.h>
#include <gdiplus.h>
#pragma comment(lib, "gdiplus.lib")
#include <iostream>
#include <string>
#include <unordered_map>
#include <Wtsapi32.h>

#include "3RVX.h"
#include "DisplayManager.h"
#include "HotkeyInfo.h"
#include "HotkeyManager.h"
#include "Logger.h"
#include "MediaKeys.h"
#include "OSD\EjectOSD.h"
#include "OSD\VolumeOSD.h"
#include "Settings.h"
#include "SkinManager.h"

HANDLE mutex;
HINSTANCE hInst;
ULONG_PTR gdiplusToken;
HWND mainWnd;

VolumeOSD *vOSD;
EjectOSD *eOSD;

HotkeyManager *hkManager;
std::unordered_map<int, HotkeyInfo> hotkeys;

void init();
HWND CreateMainWnd(HINSTANCE hInstance);
void ProcessHotkeys(HotkeyInfo &hki);
LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam);

int WINAPI wWinMain(_In_ HINSTANCE hInstance, _In_opt_ HINSTANCE hPrevInstance,
    _In_ LPWSTR lpCmdLine, _In_ int nShowCmd) {

    hInst = hInstance;

    Logger::Start();

    QCLOG(L"  _____ ______     ____  _______ ");
    QCLOG(L" |___ /|  _ \\ \\   / /\\ \\/ /___ / ");
    QCLOG(L"   |_ \\| |_) \\ \\ / /  \\  /  |_ \\ ");
    QCLOG(L"  ___) |  _ < \\ V /   /  \\ ___) |");
    QCLOG(L" |____/|_| \\_\\ \\_/   /_/\\_\\____/ ");
    QCLOG(L"");

    QCLOG(L"Starting up...");

    mutex = CreateMutex(NULL, FALSE, L"Local\\3RVX");
    if (GetLastError() == ERROR_ALREADY_EXISTS) {
        if (mutex) {
            ReleaseMutex(mutex);
        }

        HWND masterWnd = FindWindow(CLASS_3RVX, CLASS_3RVX);
        CLOG(L"A previous instance of the program is already running.\n"
            L"Requesting Settings dialog from window: %d", (int) masterWnd);
        SendMessage(masterWnd, WM_3RVX_CONTROL, MSG_SETTINGS, NULL);

#if defined(ENABLE_3RVX_LOG) && (defined(ENABLE_3RVX_LOGTOFILE) == FALSE)
        CLOG(L"Press [enter] to terminate");
        std::cin.get();
#endif

        return EXIT_SUCCESS;
    }

    CLOG(L"App directory: %s", Settings::AppDir().c_str());

    using namespace Gdiplus;
    GdiplusStartupInput gdiplusStartupInput;
    GdiplusStartup(&gdiplusToken, &gdiplusStartupInput, NULL);

    mainWnd = CreateMainWnd(hInstance);
    if (mainWnd == NULL) {
        CLOG(L"Could not create main window");
        return EXIT_FAILURE;
    }

    HRESULT hr = CoInitializeEx(NULL,
        COINIT_APARTMENTTHREADED | COINIT_DISABLE_OLE1DDE);
    if (hr != S_OK) {
        CLOG(L"Failed to initialize the COM library.");
        return EXIT_FAILURE;
    }

    /* Tell the program to initialize */
    PostMessage(mainWnd, WM_3RVX_CONTROL, MSG_LOAD, NULL);

    /* Start the event loop */
    MSG msg;
    while (GetMessage(&msg, NULL, 0, 0)) {
        TranslateMessage(&msg);
        DispatchMessage(&msg);
    }

    GdiplusShutdown(gdiplusToken);
    CoUninitialize();

    Logger::Stop();

    return (int) msg.wParam;
}

void init() {
    CLOG(L"Initializing...");

    delete vOSD;
    delete eOSD;

    Settings *settings = Settings::Instance();
    settings->Load();

    SkinManager::Instance()->LoadSkin(settings->SkinXML());

    /* TODO: Detect monitor changes, update this map, and reload/reorg OSDs */
    DisplayManager::UpdateMonitorMap();

    /* OSDs */
    eOSD = new EjectOSD();
    vOSD = new VolumeOSD();

    /* Hotkey setup */
    if (hkManager != NULL) {
        hkManager->Shutdown();
    }
    hkManager = HotkeyManager::Instance(mainWnd);

    hotkeys = Settings::Instance()->Hotkeys();
    for (auto it = hotkeys.begin(); it != hotkeys.end(); ++it) {
        int combination = it->first;
        hkManager->Register(combination);
    }

    WTSRegisterSessionNotification(mainWnd, NOTIFY_FOR_THIS_SESSION);
}

HWND CreateMainWnd(HINSTANCE hInstance) {
    WNDCLASSEX wcex;

    wcex.cbSize = sizeof(WNDCLASSEX);
    wcex.style = NULL;
    wcex.lpfnWndProc = WndProc;
    wcex.cbClsExtra = NULL;
    wcex.cbWndExtra = NULL;
    wcex.hInstance = hInstance;
    wcex.hIcon = LoadIcon(NULL, IDI_APPLICATION);
    wcex.hCursor = LoadCursor(NULL, IDC_ARROW);
    wcex.hbrBackground = (HBRUSH)(COLOR_WINDOW + 1);
    wcex.lpszMenuName = NULL;
    wcex.lpszClassName = CLASS_3RVX;
    wcex.hIconSm = LoadIcon(NULL, IDI_APPLICATION);

    if (!RegisterClassEx(&wcex)) {
        return NULL;
    }

    HWND hWnd = CreateWindowEx(
        NULL,
        CLASS_3RVX, CLASS_3RVX,
        NULL, NULL, NULL, //your boat, gently down the stream
        NULL, NULL, NULL, NULL, hInstance, NULL);

    return hWnd;
}

void ProcessHotkeys(HotkeyInfo &hki) {
    switch (hki.action) {
    case HotkeyInfo::IncreaseVolume:
    case HotkeyInfo::DecreaseVolume:
    case HotkeyInfo::SetVolume:
    case HotkeyInfo::Mute:
    case HotkeyInfo::VolumeSlider:
        if (vOSD) {
            vOSD->ProcessHotkeys(hki);
        }
        break;

    case HotkeyInfo::EjectDrive:
    case HotkeyInfo::EjectLatestDrive:
        if (eOSD) {
            eOSD->ProcessHotkeys(hki);
        }
        break;

    case HotkeyInfo::MediaKey:
        MediaKeys::ProcessHotkeys(hki);
        break;

    case HotkeyInfo::Settings:
        ShellExecute(NULL, L"open",
            Settings::SettingsApp().c_str(), NULL, NULL, SW_SHOWNORMAL);
        break;

    case HotkeyInfo::Exit:
        SendMessage(mainWnd, WM_CLOSE, NULL, NULL);
        break;
    }
}

LRESULT CALLBACK WndProc(
    HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam) {

    switch (message) {
    case WM_HOTKEY: {
        CLOG(L"Hotkey: %d", (int) wParam);
        HotkeyInfo hki = hotkeys[(int) wParam];
        ProcessHotkeys(hki);
        break;
    }

    case WM_WTSSESSION_CHANGE: {
        CLOG(L"Detected session change");
        break;
    }

    case WM_CLOSE: {
        CLOG(L"Shutting down");
        HotkeyManager::Instance()->Shutdown();
        vOSD->HideIcon();
        DestroyWindow(mainWnd);
        break;
    }

    case WM_DESTROY: {
        PostQuitMessage(0);
        break;
    }
    }

    if (message == WM_3RVX_CONTROL) {
        switch (wParam) {
        case MSG_LOAD:
            init();
            break;

        case MSG_SETTINGS:
            Settings::LaunchSettingsApp();
            break;

        case MSG_HIDEOSD:
            int except = (OSDType) lParam;
            switch (except) {
            case Volume:
                if (eOSD) {
                    eOSD->Hide();
                }
                break;

            case Eject:
                if (vOSD) {
                    vOSD->Hide();
                }
                break;
            }

            break;
        }
    }

    return DefWindowProc(hWnd, message, wParam, lParam);
}<|endoftext|>


<commit_before>#include <Windows.h>
#include <gdiplus.h>
#pragma comment(lib, "gdiplus.lib")
#include <Dbt.h>
#include <iostream>
#include <string>
#include <unordered_map>
#include <Wtsapi32.h>

#include "3RVX.h"
#include "Controllers\Volume\CoreAudio.h"
#include "Controllers\Volume\VolumeController.h"
#include "HotkeyManager.h"
#include "Logger.h"
#include "Settings.h"
#include "Skin.h"
#include "VolumeOSD.h"

HANDLE mutex;
HINSTANCE hInst;
ULONG_PTR gdiplusToken;
HWND mainWnd;

CoreAudio *volCtrl;
VolumeOSD *vOsd;
std::unordered_map<int, int> hotkeys;

void init();
HWND CreateMainWnd(HINSTANCE hInstance);
LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam);

int APIENTRY
wWinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance,
LPTSTR lpCmdLine, int nCmdShow) {
    hInst = hInstance;

#ifdef _DEBUG
    Logger::OpenConsole();
#endif

    QCLOG(L"  _____ ______     ____  _______ ");
    QCLOG(L" |___ /|  _ \\ \\   / /\\ \\/ /___ / ");
    QCLOG(L"   |_ \\| |_) \\ \\ / /  \\  /  |_ \\ ");
    QCLOG(L"  ___) |  _ < \\ V /   /  \\ ___) |");
    QCLOG(L" |____/|_| \\_\\ \\_/   /_/\\_\\____/ ");
    QCLOG(L"");

    QCLOG(L"Starting up...");

    mutex = CreateMutex(NULL, FALSE, L"Local\\3RVX");
    if (GetLastError() == ERROR_ALREADY_EXISTS) {
        if (mutex) {
            ReleaseMutex(mutex);
        }

        HWND masterWnd = FindWindow(CLASS_3RVX, CLASS_3RVX);
        CLOG(L"A previous instance of the program is already running.\n"
            L"Requesting Settings dialog from window: %d", masterWnd);
        SendMessage(masterWnd, WM_3RVX_CONTROL, MSG_SETTINGS, NULL);

#ifdef _DEBUG
        CLOG(L"Press [enter] to terminate");
        std::cin.get();
#endif

        return EXIT_SUCCESS;
    }

    CLOG(L"App directory: %s", Settings::AppDir().c_str());

    using namespace Gdiplus;
    GdiplusStartupInput gdiplusStartupInput;
    GdiplusStartup(&gdiplusToken, &gdiplusStartupInput, NULL);

    mainWnd = CreateMainWnd(hInstance);
    if (mainWnd == NULL) {
        CLOG(L"Could not create main window");
        return EXIT_FAILURE;
    }

    HRESULT hr = CoInitializeEx(NULL,
        COINIT_APARTMENTTHREADED | COINIT_DISABLE_OLE1DDE);
    if (hr != S_OK) {
        CLOG(L"Failed to initialize the COM library.");
        return EXIT_FAILURE;
    }

    /* Tell the program to initialize */
    PostMessage(mainWnd, WM_3RVX_CONTROL, MSG_LOAD, NULL);

    /* Start the event loop */
    MSG msg;
    while (GetMessage(&msg, NULL, 0, 0)) {
        TranslateMessage(&msg);
        DispatchMessage(&msg);
    }

    GdiplusShutdown(gdiplusToken);
    return (int)msg.wParam;
}

void init() {
    CLOG(L"Initializing...");

    volCtrl = new CoreAudio(mainWnd);
    volCtrl->Init();
    float currentVolume = volCtrl->Volume();

    Settings settings(L"Settings.xml");

    std::wstring skinName = settings.SkinName();
    std::wstring skinXML = Settings::AppDir() + L"\\" + SKINS_DIR L"\\"
        + skinName+ L"\\" SKIN_XML;
    Skin skin(skinXML);

    vOsd = new VolumeOSD(hInst);
    vOsd->LoadSkin(&skin);
    vOsd->MeterLevels(currentVolume);

    hotkeys = settings.Hotkeys();
    HotkeyManager *hkm = HotkeyManager::Instance(mainWnd);
    for (auto it = hotkeys.begin(); it != hotkeys.end(); ++it) {
        int combination = it->first;
        hkm->Register(combination);
    }

    WTSRegisterSessionNotification(mainWnd, NOTIFY_FOR_THIS_SESSION);
}

HWND CreateMainWnd(HINSTANCE hInstance) {
    WNDCLASSEX wcex;

    wcex.cbSize = sizeof(WNDCLASSEX);
    wcex.style = NULL;
    wcex.lpfnWndProc = WndProc;
    wcex.cbClsExtra = NULL;
    wcex.cbWndExtra = NULL;
    wcex.hInstance = hInstance;
    wcex.hIcon = LoadIcon(NULL, IDI_APPLICATION);
    wcex.hCursor = LoadCursor(NULL, IDC_ARROW);
    wcex.hbrBackground = (HBRUSH)(COLOR_WINDOW + 1);
    wcex.lpszMenuName = NULL;
    wcex.lpszClassName = CLASS_3RVX;
    wcex.hIconSm = LoadIcon(NULL, IDI_APPLICATION);

    if (!RegisterClassEx(&wcex)) {
        return NULL;
    }

    HWND hWnd = CreateWindowEx(
        NULL,
        CLASS_3RVX, CLASS_3RVX,
        NULL, NULL, NULL, //your boat, gently down the stream
        NULL, NULL, NULL, NULL, hInstance, NULL);

    return hWnd;
}

LRESULT CALLBACK WndProc(
    HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam) {

    switch (message) {
    case MSG_VOL_CHNG: {
        float v = volCtrl->Volume();
        QCLOG(L"Volume level: %.0f", v * 100.0f);
        vOsd->MeterLevels(v);
        break;
    }

    case MSG_VOL_DEVCHNG: {
        CLOG(L"Device change detected.");
        volCtrl->SelectDefaultDevice();
        break;
    }

    case WM_DEVICECHANGE: {
        if (wParam == DBT_DEVICEREMOVECOMPLETE) {
            CLOG(L"Device removal notification received");
        }
        break;
    }

    case WM_HOTKEY: {
        CLOG(L"Hotkey: %d", (int) wParam);
        int action = hotkeys[(int) wParam];
        if (action == 100) {
            float current = volCtrl->Volume();
            volCtrl->Volume(current + .1f);
        }
        break;
    }

    case WM_WTSSESSION_CHANGE: {
        CLOG(L"Detected session change");
        break;
    }

    case WM_CLOSE: {
        CLOG(L"Shutting down");
        HotkeyManager::Instance()->Shutdown();
        vOsd->HideIcon();
        DestroyWindow(mainWnd);
        break;
    }

    case WM_DESTROY: {
        PostQuitMessage(0);
        break;
    }

    }

    if (message == WM_3RVX_CONTROL) {
        switch (wParam) {
        case MSG_LOAD:
            init();
            break;

        case MSG_SETTINGS:
            CLOG(L"Launching settings editor");
            /* TODO: launch! */
            break;
        }
    }

    return DefWindowProc(hWnd, message, wParam, lParam);
}<commit_msg>Remove volume control handling/notifications<commit_after>#include <Windows.h>
#include <gdiplus.h>
#pragma comment(lib, "gdiplus.lib")
#include <Dbt.h>
#include <iostream>
#include <string>
#include <unordered_map>
#include <Wtsapi32.h>

#include "3RVX.h"
#include "HotkeyManager.h"
#include "Logger.h"
#include "Settings.h"
#include "Skin.h"
#include "VolumeOSD.h"

HANDLE mutex;
HINSTANCE hInst;
ULONG_PTR gdiplusToken;
HWND mainWnd;

VolumeOSD *vOsd;
std::unordered_map<int, int> hotkeys;

void init();
HWND CreateMainWnd(HINSTANCE hInstance);
LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam);

int APIENTRY
wWinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance,
LPTSTR lpCmdLine, int nCmdShow) {
    hInst = hInstance;

#ifdef _DEBUG
    Logger::OpenConsole();
#endif

    QCLOG(L"  _____ ______     ____  _______ ");
    QCLOG(L" |___ /|  _ \\ \\   / /\\ \\/ /___ / ");
    QCLOG(L"   |_ \\| |_) \\ \\ / /  \\  /  |_ \\ ");
    QCLOG(L"  ___) |  _ < \\ V /   /  \\ ___) |");
    QCLOG(L" |____/|_| \\_\\ \\_/   /_/\\_\\____/ ");
    QCLOG(L"");

    QCLOG(L"Starting up...");

    mutex = CreateMutex(NULL, FALSE, L"Local\\3RVX");
    if (GetLastError() == ERROR_ALREADY_EXISTS) {
        if (mutex) {
            ReleaseMutex(mutex);
        }

        HWND masterWnd = FindWindow(CLASS_3RVX, CLASS_3RVX);
        CLOG(L"A previous instance of the program is already running.\n"
            L"Requesting Settings dialog from window: %d", masterWnd);
        SendMessage(masterWnd, WM_3RVX_CONTROL, MSG_SETTINGS, NULL);

#ifdef _DEBUG
        CLOG(L"Press [enter] to terminate");
        std::cin.get();
#endif

        return EXIT_SUCCESS;
    }

    CLOG(L"App directory: %s", Settings::AppDir().c_str());

    using namespace Gdiplus;
    GdiplusStartupInput gdiplusStartupInput;
    GdiplusStartup(&gdiplusToken, &gdiplusStartupInput, NULL);

    mainWnd = CreateMainWnd(hInstance);
    if (mainWnd == NULL) {
        CLOG(L"Could not create main window");
        return EXIT_FAILURE;
    }

    HRESULT hr = CoInitializeEx(NULL,
        COINIT_APARTMENTTHREADED | COINIT_DISABLE_OLE1DDE);
    if (hr != S_OK) {
        CLOG(L"Failed to initialize the COM library.");
        return EXIT_FAILURE;
    }

    /* Tell the program to initialize */
    PostMessage(mainWnd, WM_3RVX_CONTROL, MSG_LOAD, NULL);

    /* Start the event loop */
    MSG msg;
    while (GetMessage(&msg, NULL, 0, 0)) {
        TranslateMessage(&msg);
        DispatchMessage(&msg);
    }

    GdiplusShutdown(gdiplusToken);
    return (int)msg.wParam;
}

void init() {
    CLOG(L"Initializing...");

    Settings settings(L"Settings.xml");

    std::wstring skinName = settings.SkinName();
    std::wstring skinXML = Settings::AppDir() + L"\\" + SKINS_DIR L"\\"
        + skinName+ L"\\" SKIN_XML;
    Skin skin(skinXML);

    vOsd = new VolumeOSD(hInst);
    vOsd->LoadSkin(&skin);

    hotkeys = settings.Hotkeys();
    HotkeyManager *hkm = HotkeyManager::Instance(mainWnd);
    for (auto it = hotkeys.begin(); it != hotkeys.end(); ++it) {
        int combination = it->first;
        hkm->Register(combination);
    }

    WTSRegisterSessionNotification(mainWnd, NOTIFY_FOR_THIS_SESSION);
}

HWND CreateMainWnd(HINSTANCE hInstance) {
    WNDCLASSEX wcex;

    wcex.cbSize = sizeof(WNDCLASSEX);
    wcex.style = NULL;
    wcex.lpfnWndProc = WndProc;
    wcex.cbClsExtra = NULL;
    wcex.cbWndExtra = NULL;
    wcex.hInstance = hInstance;
    wcex.hIcon = LoadIcon(NULL, IDI_APPLICATION);
    wcex.hCursor = LoadCursor(NULL, IDC_ARROW);
    wcex.hbrBackground = (HBRUSH)(COLOR_WINDOW + 1);
    wcex.lpszMenuName = NULL;
    wcex.lpszClassName = CLASS_3RVX;
    wcex.hIconSm = LoadIcon(NULL, IDI_APPLICATION);

    if (!RegisterClassEx(&wcex)) {
        return NULL;
    }

    HWND hWnd = CreateWindowEx(
        NULL,
        CLASS_3RVX, CLASS_3RVX,
        NULL, NULL, NULL, //your boat, gently down the stream
        NULL, NULL, NULL, NULL, hInstance, NULL);

    return hWnd;
}

LRESULT CALLBACK WndProc(
    HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam) {

    switch (message) {
    case WM_DEVICECHANGE: {
        if (wParam == DBT_DEVICEREMOVECOMPLETE) {
            CLOG(L"Device removal notification received");
        }
        break;
    }

    case WM_HOTKEY: {
        CLOG(L"Hotkey: %d", (int) wParam);
        int action = hotkeys[(int) wParam];
        break;
    }

    case WM_WTSSESSION_CHANGE: {
        CLOG(L"Detected session change");
        break;
    }

    case WM_CLOSE: {
        CLOG(L"Shutting down");
        HotkeyManager::Instance()->Shutdown();
        vOsd->HideIcon();
        DestroyWindow(mainWnd);
        break;
    }

    case WM_DESTROY: {
        PostQuitMessage(0);
        break;
    }
    }

    if (message == WM_3RVX_CONTROL) {
        switch (wParam) {
        case MSG_LOAD:
            init();
            break;

        case MSG_SETTINGS:
            CLOG(L"Launching settings editor");
            /* TODO: launch! */
            break;
        }
    }

    return DefWindowProc(hWnd, message, wParam, lParam);
}<|endoftext|>


<commit_before>#include "Engine.h"
#include "Game.h"
#include "ObjectDummy.h"
#include "BodyRigid.h"
#include "BodyDummy.h"

#include "Physics.h"
#include "ShapeCapsule.h"
#include "ActorBase.h"
#include "Visualizer.h"
#include "sys/SysControl.h"

#ifdef MEMORY_INFO
#define new new(__FILE__, __LINE__)
#endif // MEMORY_INFO

using namespace MathLib;

/*
*/
#define ACTOR_BASE_IFPS             (1.0f / 60.0f)
#define ACTOR_BASE_CLAMP            89.9f
#define ACTOR_BASE_COLLISIONS       4

/*
*/

CActorBase::CActorBase()
{
	m_vUp = vec3(0.0f, 0.0f, 1.0f);
	m_pObject = new CObjectDummy();
	m_pDummy = new CBodyDummy();
	m_pShape = new CShapeCapsule(1.0f, 1.0f);

	m_nFlush = 0;
	m_vPosition = Vec3_zero;
	m_vVelocity = Vec3_zero;
	m_fPhiAngle = 0.0f;			//б		άƽϵУֱYķX֮ļн
	m_fThetaAngle = 0.0f;			//λǣ뵱ǰ˳ʱ߹ĽǶ

	for (int i = 0; i < NUM_STATES; i++)
	{
		m_pStates[i] = 0;
		m_pTimes[i] = 0.0f;
	}

	m_pDummy->SetEnabled(1);
	m_pObject->SetBody(NULL);
	m_pObject->SetBody(m_pDummy);
	m_pShape->SetBody(NULL);
	m_pShape->SetBody(m_pDummy);

	m_pObject->SetWorldTransform(Get_Body_Transform());
	m_pShape->SetRestitution(0.0f);
	m_pShape->SetCollisionMask(2);

	SetEnabled(1);
	SetViewDirection(vec3(0.0f,1.0f,0.0f));
	SetCollision(1);
	SetCollisionRadius(0.3f);
	SetCollisionHeight(1.0f);
	SetFriction(2.0f);
	SetMinVelocity(2.0f);
	SetMaxVelocity(4.0f);
	SetAcceleration(8.0f);
	SetDamping(8.0f);
	SetJumping(1.5f);

	SetGround(0);
	SetCeiling(0);

}
CActorBase::~CActorBase()
{
	m_pDummy->SetObject(NULL);
	delete m_pObject;
	delete m_pDummy;
}

void CActorBase::SetEnabled(int enable)
{
	m_nEnable = enable;
	m_pDummy->SetEnabled(m_nEnable);
}
int CActorBase::IsEnabled() const
{
	return m_nEnable;
}
void CActorBase::Update(float ifps)
{
	if (!m_nEnable)
	{
		return;
	}
	// impulse
	vec3 impulse = vec3_zero;

	// ortho basis
	vec3 tangent, binormal;
	OrthoBasis(m_vUp, tangent, binormal);
	// current basis
	vec3 x = quat(m_vUp, -m_fPhiAngle) * binormal;
	vec3 y = Normalize(Cross(m_vUp, x));
	vec3 z = Normalize(Cross(x, y));
	// handle states
	Update_States(1, ifps);

	// old velocity
	float x_velocity = Dot(x, m_vVelocity);
	float y_velocity = Dot(y, m_vVelocity);
	float z_velocity = Dot(z, m_vVelocity);

	// movement
	if (m_pStates[STATE_FORWARD]) impulse += x;
	if (m_pStates[STATE_BACKWARD]) impulse -= x;
	if (m_pStates[STATE_MOVE_LEFT]) impulse += y;
	if (m_pStates[STATE_MOVE_RIGHT]) impulse -= y;
	impulse.normalize();
	// velocity
	if (m_pStates[STATE_RUN]) impulse *= m_fMaxVelocity;
	else impulse *= m_fMinVelocity;

	// jump
	if (m_pStates[STATE_JUMP] == STATE_BEGIN)
	{
		impulse += z * CMathCore::Sqrt(2.0f * 9.8f * m_fJumping) / (m_fAcceleration * ifps);
	}

	// rotate velocity
	if (GetGround())
	{
		m_vVelocity = x * x_velocity + y * y_velocity + z * z_velocity;
	}
	// time
	float time = ifps * g_Engine.pPhysics->GetScale();
	// target velocity
	float target_velocity = Length(vec2(Dot(x, impulse), Dot(y, impulse)));

	// penetration tolerance
	float penetration = g_Engine.pPhysics->GetPenetrationTolerance();
	float penetration_2 = penetration * 2.0f;

	// frozen linear velocity
	float frozen_velocity = g_Engine.pPhysics->GetFrozenLinearVelocity();

	// friction
	float friction = 0.0f;
	if (target_velocity < EPSILON)
	{
		friction = m_fFriction;
	}

	// clear collision flags
	if (GetCollision())
	{
		m_nGround = 0;
		m_nCeiling = 0;
	}

	// movement
	do
	{
		// adaptive time step
		float ifps = Min(time, ACTOR_BASE_IFPS);
		time -= ifps;
		// save old velocity
		float old_velocity = Length(vec2(Dot(x, m_vVelocity), Dot(y, m_vVelocity)));

		// integrate velocity
		m_vVelocity += impulse * (m_fAcceleration * ifps);
		m_vVelocity += g_Engine.pPhysics->GetGravity() * ifps;

		// damping
		float current_velocity = Length(vec2(Dot(x, m_vVelocity), Dot(y, m_vVelocity)));
		if (target_velocity < EPSILON || current_velocity > target_velocity)
		{
			m_vVelocity = (x * Dot(x, m_vVelocity) + y * Dot(y, m_vVelocity)) * CMathCore::Exp(-m_fDamping * ifps) + z * Dot(z, m_vVelocity);
		}

		// clamp maximum velocity
		current_velocity = Length(vec2(Dot(x, m_vVelocity), Dot(y, m_vVelocity)));

		if (current_velocity > old_velocity)
		{
			if (current_velocity > target_velocity)
			{
				m_vVelocity = (x * Dot(x, m_vVelocity) + y * Dot(y, m_vVelocity)) * target_velocity / current_velocity + z * Dot(z, m_vVelocity);
			}
		}
		// integrate position
		//m_vPosition += Vec3(m_vVelocity * ifps);
		if (GetCollision())
		{
			// get collision
			vec3 tangent, binormal;
			const Vec3 *caps = m_pShape->GetCaps();
			for (int i = 0; i < ACTOR_BASE_COLLISIONS; i++)
			{
				m_pDummy->SetTransform(Get_Body_Transform());
				m_pShape->GetCollision(m_vecContacts, 0.0f);

				if (m_vecContacts.Size() == 0) break;
				float inum_contacts = 1.0f / CMathCore::Itof(m_vecContacts.Size());
				for (int j = 0; j < m_vecContacts.Size(); j++)
				{
					const CShape::Contact &c = m_vecContacts[j];

					vec3 normalCollision = c.normal;
					if (is_frozen && c.depth < penetration_2)
					{
						m_vPosition += Vec3(z * (Max(c.depth - penetration, 0.0f) * inum_contacts * Dot(z, normalCollision)));
					}
					else
					{
						m_vPosition += Vec3(normalCollision * (Max(c.depth - penetration, 0.0f) * inum_contacts));
						is_frozen = 0;
					}
					float normal_velocity = Dot(normalCollision, m_vVelocity);
					if (normal_velocity < 0.0f)
					{
						m_vVelocity -= normalCollision * normal_velocity;
					}
					if (friction > EPSILON)
					{
						OrthoBasis(c.normal, tangent, binormal);
						float tangent_velocity = Dot(tangent, m_vVelocity);
						float binormal_velocity = Dot(binormal, m_vVelocity);
						if (CMathCore::Abs(tangent_velocity) > EPSILON || CMathCore::Abs(binormal_velocity) > EPSILON) {

					}

				}

			}
		}
	}

}<commit_msg>Signed-off-by: mrlitong <litongtongxue@gmail.com><commit_after>#include "Engine.h"
#include "Game.h"
#include "ObjectDummy.h"
#include "BodyRigid.h"
#include "BodyDummy.h"

#include "Physics.h"
#include "ShapeCapsule.h"
#include "ActorBase.h"
#include "Visualizer.h"
#include "sys/SysControl.h"

#ifdef MEMORY_INFO
#define new new(__FILE__, __LINE__)
#endif // MEMORY_INFO

using namespace MathLib;

/*
*/
#define ACTOR_BASE_IFPS             (1.0f / 60.0f)
#define ACTOR_BASE_CLAMP            89.9f
#define ACTOR_BASE_COLLISIONS       4

/*
*/

CActorBase::CActorBase()
{
	m_vUp = vec3(0.0f, 0.0f, 1.0f);
	m_pObject = new CObjectDummy();
	m_pDummy = new CBodyDummy();
	m_pShape = new CShapeCapsule(1.0f, 1.0f);

	m_nFlush = 0;
	m_vPosition = Vec3_zero;
	m_vVelocity = Vec3_zero;
	m_fPhiAngle = 0.0f;			//б		άƽϵУֱYķX֮ļн
	m_fThetaAngle = 0.0f;			//λǣ뵱ǰ˳ʱ߹ĽǶ

	for (int i = 0; i < NUM_STATES; i++)
	{
		m_pStates[i] = 0;
		m_pTimes[i] = 0.0f;
	}

	m_pDummy->SetEnabled(1);
	m_pObject->SetBody(NULL);
	m_pObject->SetBody(m_pDummy);
	m_pShape->SetBody(NULL);
	m_pShape->SetBody(m_pDummy);

	m_pObject->SetWorldTransform(Get_Body_Transform());
	m_pShape->SetRestitution(0.0f);
	m_pShape->SetCollisionMask(2);

	SetEnabled(1);
	SetViewDirection(vec3(0.0f,1.0f,0.0f));
	SetCollision(1);
	SetCollisionRadius(0.3f);
	SetCollisionHeight(1.0f);
	SetFriction(2.0f);
	SetMinVelocity(2.0f);
	SetMaxVelocity(4.0f);
	SetAcceleration(8.0f);
	SetDamping(8.0f);
	SetJumping(1.5f);

	SetGround(0);
	SetCeiling(0);

}
CActorBase::~CActorBase()
{
	m_pDummy->SetObject(NULL);
	delete m_pObject;
	delete m_pDummy;
}

void CActorBase::SetEnabled(int enable)
{
	m_nEnable = enable;
	m_pDummy->SetEnabled(m_nEnable);
}
int CActorBase::IsEnabled() const
{
	return m_nEnable;
}
void CActorBase::Update(float ifps)
{
	if (!m_nEnable)
	{
		return;
	}
	// impulse
	vec3 impulse = vec3_zero;

	// ortho basis
	vec3 tangent, binormal;
	OrthoBasis(m_vUp, tangent, binormal);
	// current basis
	vec3 x = quat(m_vUp, -m_fPhiAngle) * binormal;
	vec3 y = Normalize(Cross(m_vUp, x));
	vec3 z = Normalize(Cross(x, y));
	// handle states
	Update_States(1, ifps);

	// old velocity
	float x_velocity = Dot(x, m_vVelocity);
	float y_velocity = Dot(y, m_vVelocity);
	float z_velocity = Dot(z, m_vVelocity);

	// movement
	if (m_pStates[STATE_FORWARD]) impulse += x;
	if (m_pStates[STATE_BACKWARD]) impulse -= x;
	if (m_pStates[STATE_MOVE_LEFT]) impulse += y;
	if (m_pStates[STATE_MOVE_RIGHT]) impulse -= y;
	impulse.normalize();
	// velocity
	if (m_pStates[STATE_RUN]) impulse *= m_fMaxVelocity;
	else impulse *= m_fMinVelocity;

	// jump
	if (m_pStates[STATE_JUMP] == STATE_BEGIN)
	{
		impulse += z * CMathCore::Sqrt(2.0f * 9.8f * m_fJumping) / (m_fAcceleration * ifps);
	}

	// rotate velocity
	if (GetGround())
	{
		m_vVelocity = x * x_velocity + y * y_velocity + z * z_velocity;
	}
	// time
	float time = ifps * g_Engine.pPhysics->GetScale();
	// target velocity
	float target_velocity = Length(vec2(Dot(x, impulse), Dot(y, impulse)));

	// penetration tolerance
	float penetration = g_Engine.pPhysics->GetPenetrationTolerance();
	float penetration_2 = penetration * 2.0f;

	// frozen linear velocity
	float frozen_velocity = g_Engine.pPhysics->GetFrozenLinearVelocity();

	// friction
	float friction = 0.0f;
	if (target_velocity < EPSILON)
	{
		friction = m_fFriction;
	}

	// clear collision flags
	if (GetCollision())
	{
		m_nGround = 0;
		m_nCeiling = 0;
	}

	// movement
	do
	{
		// adaptive time step
		float ifps = Min(time, ACTOR_BASE_IFPS);
		time -= ifps;
		// save old velocity
		float old_velocity = Length(vec2(Dot(x, m_vVelocity), Dot(y, m_vVelocity)));

		// integrate velocity
		m_vVelocity += impulse * (m_fAcceleration * ifps);
		m_vVelocity += g_Engine.pPhysics->GetGravity() * ifps;

		// damping
		float current_velocity = Length(vec2(Dot(x, m_vVelocity), Dot(y, m_vVelocity)));
		if (target_velocity < EPSILON || current_velocity > target_velocity)
		{
			m_vVelocity = (x * Dot(x, m_vVelocity) + y * Dot(y, m_vVelocity)) * CMathCore::Exp(-m_fDamping * ifps) + z * Dot(z, m_vVelocity);
		}

		// clamp maximum velocity
		current_velocity = Length(vec2(Dot(x, m_vVelocity), Dot(y, m_vVelocity)));

		if (current_velocity > old_velocity)
		{
			if (current_velocity > target_velocity)
			{
				m_vVelocity = (x * Dot(x, m_vVelocity) + y * Dot(y, m_vVelocity)) * target_velocity / current_velocity + z * Dot(z, m_vVelocity);
			}
		}
		// integrate position
		//m_vPosition += Vec3(m_vVelocity * ifps);
		if (GetCollision())
		{
			// get collision
			vec3 tangent, binormal;
			const Vec3 *caps = m_pShape->GetCaps();
			for (int i = 0; i < ACTOR_BASE_COLLISIONS; i++)
			{
				m_pDummy->SetTransform(Get_Body_Transform());
				m_pShape->GetCollision(m_vecContacts, 0.0f);

				if (m_vecContacts.Size() == 0) break;
				float inum_contacts = 1.0f / CMathCore::Itof(m_vecContacts.Size());
				for (int j = 0; j < m_vecContacts.Size(); j++)
				{
					const CShape::Contact &c = m_vecContacts[j];

					vec3 normalCollision = c.normal;
					if (is_frozen && c.depth < penetration_2)
					{
						m_vPosition += Vec3(z * (Max(c.depth - penetration, 0.0f) * inum_contacts * Dot(z, normalCollision)));
					}
					else
					{
						m_vPosition += Vec3(normalCollision * (Max(c.depth - penetration, 0.0f) * inum_contacts));
						is_frozen = 0;
					}
					float normal_velocity = Dot(normalCollision, m_vVelocity);
					if (normal_velocity < 0.0f)
					{
						m_vVelocity -= normalCollision * normal_velocity;
					}
					if (friction > EPSILON)
					{
						OrthoBasis(c.normal, tangent, binormal);
						float tangent_velocity = Dot(tangent, m_vVelocity);
						float binormal_velocity = Dot(binormal, m_vVelocity);
						if (CMathCore::Abs(tangent_velocity) > EPSILON || CMathCore::Abs(binormal_velocity) > EPSILON)
						{

						}

					}

				}

			}
		}
	}

}<|endoftext|>


<commit_before>#include "Engine.h"
#include "Game.h"
#include "ObjectDummy.h"
#include "BodyRigid.h"
#include "BodyDummy.h"

#include "Physics.h"
#include "ShapeCapsule.h"
#include "ActorBase.h"
#include "Visualizer.h"
#include "sys/SysControl.h"

#ifdef MEMORY_INFO
#define new new(__FILE__, __LINE__)
#endif // MEMORY_INFO

using namespace MathLib;

/*
*/
#define ACTOR_BASE_IFPS             (1.0f / 60.0f)
#define ACTOR_BASE_CLAMP            89.9f
#define ACTOR_BASE_COLLISIONS       4

/*
*/

CActorBase::CActorBase()
{
	m_vUp = vec3(0.0f, 0.0f, 1.0f);
	m_pObject = new CObjectDummy();
	m_pDummy = new CBodyDummy();
	m_pShape = new CShapeCapsule(1.0f, 1.0f);

	m_nFlush = 0;
	m_vPosition = Vec3_zero;
	m_vVelocity = Vec3_zero;
	m_fPhiAngle = 0.0f;			//б		άƽϵУֱYķX֮ļн
	m_fThetaAngle = 0.0f;			//λǣ뵱ǰ˳ʱ߹ĽǶ

	for (int i = 0; i < NUM_STATES; i++)
	{
		m_pStates[i] = 0;
		m_pTimes[i] = 0.0f;
	}

	m_pDummy->SetEnabled(1);
	m_pObject->SetBody(NULL);
	m_pObject->SetBody(m_pDummy);
	m_pShape->SetBody(NULL);
	m_pShape->SetBody(m_pDummy);

	m_pObject->SetWorldTransform(Get_Body_Transform());
	m_pShape->SetRestitution(0.0f);
	m_pShape->SetCollisionMask(2);

	SetEnabled(1);
	SetViewDirection(vec3(0.0f, 1.0f, 0.0f));
	SetCollision(1);
	SetCollisionRadius(0.3f);
	SetCollisionHeight(1.0f);
	SetFriction(2.0f);
	SetMinVelocity(2.0f);
	SetMaxVelocity(4.0f);
	SetAcceleration(8.0f);
	SetDamping(8.0f);
	SetJumping(1.5f);

	SetGround(0);
	SetCeiling(0);

}
CActorBase::~CActorBase()
{
	m_pDummy->SetObject(NULL);
	delete m_pObject;
	delete m_pDummy;
}

void CActorBase::SetEnabled(int enable)
{
	m_nEnable = enable;
	m_pDummy->SetEnabled(m_nEnable);
}
int CActorBase::IsEnabled() const
{
	return m_nEnable;
}
void CActorBase::Update(float ifps)
{
	if (!m_nEnable)
	{
		return;
	}
	// impulse
	vec3 impulse = vec3_zero;

	// ortho basis
	vec3 tangent, binormal;
	OrthoBasis(m_vUp, tangent, binormal);
	// current basis
	vec3 x = quat(m_vUp, -m_fPhiAngle) * binormal;
	vec3 y = Normalize(Cross(m_vUp, x));
	vec3 z = Normalize(Cross(x, y));
	// handle states
	Update_States(1, ifps);

	// old velocity
	float x_velocity = Dot(x, m_vVelocity);
	float y_velocity = Dot(y, m_vVelocity);
	float z_velocity = Dot(z, m_vVelocity);

	// movement
	if (m_pStates[STATE_FORWARD]) impulse += x;
	if (m_pStates[STATE_BACKWARD]) impulse -= x;
	if (m_pStates[STATE_MOVE_LEFT]) impulse += y;
	if (m_pStates[STATE_MOVE_RIGHT]) impulse -= y;
	impulse.normalize();
	// velocity
	if (m_pStates[STATE_RUN]) impulse *= m_fMaxVelocity;
	else impulse *= m_fMinVelocity;

	// jump
	if (m_pStates[STATE_JUMP] == STATE_BEGIN)
	{
		impulse += z * CMathCore::Sqrt(2.0f * 9.8f * m_fJumping) / (m_fAcceleration * ifps);
	}

	// rotate velocity
	if (GetGround())
	{
		m_vVelocity = x * x_velocity + y * y_velocity + z * z_velocity;
	}
	// time
	float time = ifps * g_Engine.pPhysics->GetScale();
	// target velocity
	float target_velocity = Length(vec2(Dot(x, impulse), Dot(y, impulse)));

	// penetration tolerance
	float penetration = g_Engine.pPhysics->GetPenetrationTolerance();
	float penetration_2 = penetration * 2.0f;

	// frozen linear velocity
	float frozen_velocity = g_Engine.pPhysics->GetFrozenLinearVelocity();

	// friction
	float friction = 0.0f;
	if (target_velocity < EPSILON)
	{
		friction = m_fFriction;
	}

	// clear collision flags
	if (GetCollision())
	{
		m_nGround = 0;
		m_nCeiling = 0;
	}

	// movement
	do
	{
		// adaptive time step
		float ifps = Min(time, ACTOR_BASE_IFPS);
		time -= ifps;
		// save old velocity
		float old_velocity = Length(vec2(Dot(x, m_vVelocity), Dot(y, m_vVelocity)));

		// integrate velocity
		m_vVelocity += impulse * (m_fAcceleration * ifps);
		m_vVelocity += g_Engine.pPhysics->GetGravity() * ifps;

		// damping
		float current_velocity = Length(vec2(Dot(x, m_vVelocity), Dot(y, m_vVelocity)));
		if (target_velocity < EPSILON || current_velocity > target_velocity)
		{
			m_vVelocity = (x * Dot(x, m_vVelocity) + y * Dot(y, m_vVelocity)) * CMathCore::Exp(-m_fDamping * ifps) + z * Dot(z, m_vVelocity);
		}

		// clamp maximum velocity
		current_velocity = Length(vec2(Dot(x, m_vVelocity), Dot(y, m_vVelocity)));

		if (current_velocity > old_velocity)
		{
			if (current_velocity > target_velocity)
			{
				m_vVelocity = (x * Dot(x, m_vVelocity) + y * Dot(y, m_vVelocity)) * target_velocity / current_velocity + z * Dot(z, m_vVelocity);
			}
		}
		// integrate position
		//m_vPosition += Vec3(m_vVelocity * ifps);
		if (GetCollision())
		{
			// get collision
			vec3 tangent, binormal;
			const Vec3 *caps = m_pShape->GetCaps();
			for (int i = 0; i < ACTOR_BASE_COLLISIONS; i++)
			{
				m_pDummy->SetTransform(Get_Body_Transform());
				m_pShape->GetCollision(m_vecContacts, 0.0f);

				if (m_vecContacts.Size() == 0) break;
				float inum_contacts = 1.0f / CMathCore::Itof(m_vecContacts.Size());
				for (int j = 0; j < m_vecContacts.Size(); j++)
				{
					const CShape::Contact &c = m_vecContacts[j];

					vec3 normalCollision = c.normal;
					if (is_frozen && c.depth < penetration_2)
					{
						m_vPosition += Vec3(z * (Max(c.depth - penetration, 0.0f) * inum_contacts * Dot(z, normalCollision)));
					}
					else
					{
						m_vPosition += Vec3(normalCollision * (Max(c.depth - penetration, 0.0f) * inum_contacts));
						is_frozen = 0;
					}
					float normal_velocity = Dot(normalCollision, m_vVelocity);
					if (normal_velocity < 0.0f)
					{
						m_vVelocity -= normalCollision * normal_velocity;
					}
					if (friction > EPSILON)
					{
						OrthoBasis(c.normal, tangent, binormal);
						float tangent_velocity = Dot(tangent, m_vVelocity);
						float binormal_velocity = Dot(binormal, m_vVelocity);
						if (CMathCore::Abs(tangent_velocity) > EPSILON || CMathCore::Abs(binormal_velocity) > EPSILON)
						{
							float friction_velocity = Clamp(Max(-normal_velocity, 0.0f) * friction * CMathCore::RSqrt(tangent_velocity * tangent_velocity + binormal_velocity * binormal_velocity), -1.0f, 1.0f);
							m_vVelocity -= tangent * tangent_velocity * friction_velocity;
							m_vVelocity -= binormal * binormal_velocity * friction_velocity;

						}
					}
					if (Dot(c.normal, m_vUp) > 0.5f && Dot(vec3(c.point - caps[0]), m_vUp) < 0.0f) m_nGround = 1;
					if (Dot(c.normal, m_vUp) < -0.5f && Dot(vec3(c.point - caps[1]), m_vUp) > 0.0f) m_nCeiling = 1;
				}
			}
		}

		m_vPosition += Vec3(m_vVelocity * ifps);
	} while (time > EPSILON);

	// current position
	m_pObject->SetWorldTransform(Get_Body_Transform());
	m_WorldBoundBox.Set(m_BoundBox, Translate(m_vPosition));
	m_WorldBoundSphere.Set(m_BoundSphere, Translate(m_vPosition));
}
/******************************************************************************\
*
* Parameters
*
\******************************************************************************/
/*
*/
void CActorBase::Update_Bounds()
{
	float radius = m_pShape->GetRadius();
	float hheight = m_pShape->GetHHeight();
	m_BoundBox.Set(vec3(-radius, -radius, 0.0f), vec3(radius, radius, (radius + hheight) * 2.0f));
	m_BoundSphere.Set(vec3(0.0f, 0.0f, radius + hheight), radius + hheight);

	m_WorldBoundBox.Set(m_BoundBox, Translate(m_vPosition));
	m_WorldBoundSphere.Set(m_BoundSphere, Translate(m_vPosition));
}
/*
*/
void CActorBase::SetIntersectionMask(int mask)
{
	m_pShape->SetIntersectionMask(mask);
}
int CActorBase::GetIntersectionMask() const
{
	return m_pShape->GetIntersectionMask();
}

/*
*/
void CActorBase::SetCollision(int c)
{
	m_nCollision = c;
}
int CActorBase::GetCollision() const
{
	return m_nCollision;
}
void CActorBase::SetCollisionMask(int mask)
{
	m_pShape->SetCollisionMask(mask);
}
int CActorBase::GetCollisionMask() const
{
	return m_pShape->GetCollisionMask();
}

/*
*/
void CActorBase::SetCollisionRadius(float radius)
{
	if (!Compare(m_pShape->GetRadius(), radius))
	{
		m_pDummy->SetPreserveTransform(Mat4(Translate(m_vUp * (radius - m_pShape->GetRadius()))) * m_pDummy->GetTransform());
		m_pShape->SetRadius(radius);
	}
	Update_Bounds();
}
float CActorBase::GetCollisionRadius() const
{
	return m_pShape->GetRadius();
}

void CActorBase::SetCollisionHeight(float height)
{
	if (!Compare(m_pShape->GetHeight(), height))
	{
		m_pDummy->SetPreserveTransform(Mat4(Translate(m_vUp * (height - m_pShape->GetHeight()) * 0.5f)) * m_pDummy->GetTransform());
		m_pShape->SetHeight(height);
	}
	Update_Bounds();
}
float CActorBase::GetCollisionHeight() const
{
	return m_pShape->GetHeight();
}

void CActorBase::SetMinVelocity(float velocity)
{
	m_fMinVelocity = Max(velocity, 0.0f);
}
float CActorBase::GetMinVelocity() const
{
	return m_fMinVelocity;
}
/*

*/
void CActorBase::SetMaxVelocity(float velocity)
{
	m_fMaxVelocity = Max(velocity, 0.0f);
}
float CActorBase::GetMaxVelocity() const
{
	return m_fMaxVelocity;
}<commit_msg>Signed-off-by: mrlitong <litongtongxue@gmail.com><commit_after>#include "Engine.h"
#include "Game.h"
#include "ObjectDummy.h"
#include "BodyRigid.h"
#include "BodyDummy.h"

#include "Physics.h"
#include "ShapeCapsule.h"
#include "ActorBase.h"
#include "Visualizer.h"
#include "sys/SysControl.h"

#ifdef MEMORY_INFO
#define new new(__FILE__, __LINE__)
#endif // MEMORY_INFO

using namespace MathLib;

/*
*/
#define ACTOR_BASE_IFPS             (1.0f / 60.0f)
#define ACTOR_BASE_CLAMP            89.9f
#define ACTOR_BASE_COLLISIONS       4

/*
*/

CActorBase::CActorBase()
{
	m_vUp = vec3(0.0f, 0.0f, 1.0f);
	m_pObject = new CObjectDummy();
	m_pDummy = new CBodyDummy();
	m_pShape = new CShapeCapsule(1.0f, 1.0f);

	m_nFlush = 0;
	m_vPosition = Vec3_zero;
	m_vVelocity = Vec3_zero;
	m_fPhiAngle = 0.0f;			//б		άƽϵУֱYķX֮ļн
	m_fThetaAngle = 0.0f;			//λǣ뵱ǰ˳ʱ߹ĽǶ

	for (int i = 0; i < NUM_STATES; i++)
	{
		m_pStates[i] = 0;
		m_pTimes[i] = 0.0f;
	}

	m_pDummy->SetEnabled(1);
	m_pObject->SetBody(NULL);
	m_pObject->SetBody(m_pDummy);
	m_pShape->SetBody(NULL);
	m_pShape->SetBody(m_pDummy);

	m_pObject->SetWorldTransform(Get_Body_Transform());
	m_pShape->SetRestitution(0.0f);
	m_pShape->SetCollisionMask(2);

	SetEnabled(1);
	SetViewDirection(vec3(0.0f, 1.0f, 0.0f));
	SetCollision(1);
	SetCollisionRadius(0.3f);
	SetCollisionHeight(1.0f);
	SetFriction(2.0f);
	SetMinVelocity(2.0f);
	SetMaxVelocity(4.0f);
	SetAcceleration(8.0f);
	SetDamping(8.0f);
	SetJumping(1.5f);

	SetGround(0);
	SetCeiling(0);

}
CActorBase::~CActorBase()
{
	m_pDummy->SetObject(NULL);
	delete m_pObject;
	delete m_pDummy;
}

void CActorBase::SetEnabled(int enable)
{
	m_nEnable = enable;
	m_pDummy->SetEnabled(m_nEnable);
}
int CActorBase::IsEnabled() const
{
	return m_nEnable;
}
void CActorBase::Update(float ifps)
{
	if (!m_nEnable)
	{
		return;
	}
	// impulse
	vec3 impulse = vec3_zero;

	// ortho basis
	vec3 tangent, binormal;
	OrthoBasis(m_vUp, tangent, binormal);
	// current basis
	vec3 x = quat(m_vUp, -m_fPhiAngle) * binormal;
	vec3 y = Normalize(Cross(m_vUp, x));
	vec3 z = Normalize(Cross(x, y));
	// handle states
	Update_States(1, ifps);

	// old velocity
	float x_velocity = Dot(x, m_vVelocity);
	float y_velocity = Dot(y, m_vVelocity);
	float z_velocity = Dot(z, m_vVelocity);

	// movement
	if (m_pStates[STATE_FORWARD]) impulse += x;
	if (m_pStates[STATE_BACKWARD]) impulse -= x;
	if (m_pStates[STATE_MOVE_LEFT]) impulse += y;
	if (m_pStates[STATE_MOVE_RIGHT]) impulse -= y;
	impulse.normalize();
	// velocity
	if (m_pStates[STATE_RUN]) impulse *= m_fMaxVelocity;
	else impulse *= m_fMinVelocity;

	// jump
	if (m_pStates[STATE_JUMP] == STATE_BEGIN)
	{
		impulse += z * CMathCore::Sqrt(2.0f * 9.8f * m_fJumping) / (m_fAcceleration * ifps);
	}

	// rotate velocity
	if (GetGround())
	{
		m_vVelocity = x * x_velocity + y * y_velocity + z * z_velocity;
	}
	// time
	float time = ifps * g_Engine.pPhysics->GetScale();
	// target velocity
	float target_velocity = Length(vec2(Dot(x, impulse), Dot(y, impulse)));

	// penetration tolerance
	float penetration = g_Engine.pPhysics->GetPenetrationTolerance();
	float penetration_2 = penetration * 2.0f;

	// frozen linear velocity
	float frozen_velocity = g_Engine.pPhysics->GetFrozenLinearVelocity();

	// friction
	float friction = 0.0f;
	if (target_velocity < EPSILON)
	{
		friction = m_fFriction;
	}

	// clear collision flags
	if (GetCollision())
	{
		m_nGround = 0;
		m_nCeiling = 0;
	}

	// movement
	do
	{
		// adaptive time step
		float ifps = Min(time, ACTOR_BASE_IFPS);
		time -= ifps;
		// save old velocity
		float old_velocity = Length(vec2(Dot(x, m_vVelocity), Dot(y, m_vVelocity)));

		// integrate velocity
		m_vVelocity += impulse * (m_fAcceleration * ifps);
		m_vVelocity += g_Engine.pPhysics->GetGravity() * ifps;

		// damping
		float current_velocity = Length(vec2(Dot(x, m_vVelocity), Dot(y, m_vVelocity)));
		if (target_velocity < EPSILON || current_velocity > target_velocity)
		{
			m_vVelocity = (x * Dot(x, m_vVelocity) + y * Dot(y, m_vVelocity)) * CMathCore::Exp(-m_fDamping * ifps) + z * Dot(z, m_vVelocity);
		}

		// clamp maximum velocity
		current_velocity = Length(vec2(Dot(x, m_vVelocity), Dot(y, m_vVelocity)));

		if (current_velocity > old_velocity)
		{
			if (current_velocity > target_velocity)
			{
				m_vVelocity = (x * Dot(x, m_vVelocity) + y * Dot(y, m_vVelocity)) * target_velocity / current_velocity + z * Dot(z, m_vVelocity);
			}
		}
		// integrate position
		//m_vPosition += Vec3(m_vVelocity * ifps);
		if (GetCollision())
		{
			// get collision
			vec3 tangent, binormal;
			const Vec3 *caps = m_pShape->GetCaps();
			for (int i = 0; i < ACTOR_BASE_COLLISIONS; i++)
			{
				m_pDummy->SetTransform(Get_Body_Transform());
				m_pShape->GetCollision(m_vecContacts, 0.0f);

				if (m_vecContacts.Size() == 0) break;
				float inum_contacts = 1.0f / CMathCore::Itof(m_vecContacts.Size());
				for (int j = 0; j < m_vecContacts.Size(); j++)
				{
					const CShape::Contact &c = m_vecContacts[j];

					vec3 normalCollision = c.normal;
					if (is_frozen && c.depth < penetration_2)
					{
						m_vPosition += Vec3(z * (Max(c.depth - penetration, 0.0f) * inum_contacts * Dot(z, normalCollision)));
					}
					else
					{
						m_vPosition += Vec3(normalCollision * (Max(c.depth - penetration, 0.0f) * inum_contacts));
						is_frozen = 0;
					}
					float normal_velocity = Dot(normalCollision, m_vVelocity);
					if (normal_velocity < 0.0f)
					{
						m_vVelocity -= normalCollision * normal_velocity;
					}
					if (friction > EPSILON)
					{
						OrthoBasis(c.normal, tangent, binormal);
						float tangent_velocity = Dot(tangent, m_vVelocity);
						float binormal_velocity = Dot(binormal, m_vVelocity);
						if (CMathCore::Abs(tangent_velocity) > EPSILON || CMathCore::Abs(binormal_velocity) > EPSILON)
						{
							float friction_velocity = Clamp(Max(-normal_velocity, 0.0f) * friction * CMathCore::RSqrt(tangent_velocity * tangent_velocity + binormal_velocity * binormal_velocity), -1.0f, 1.0f);
							m_vVelocity -= tangent * tangent_velocity * friction_velocity;
							m_vVelocity -= binormal * binormal_velocity * friction_velocity;

						}
					}
					if (Dot(c.normal, m_vUp) > 0.5f && Dot(vec3(c.point - caps[0]), m_vUp) < 0.0f) m_nGround = 1;
					if (Dot(c.normal, m_vUp) < -0.5f && Dot(vec3(c.point - caps[1]), m_vUp) > 0.0f) m_nCeiling = 1;
				}
			}
		}

		m_vPosition += Vec3(m_vVelocity * ifps);
	} while (time > EPSILON);

	// current position
	m_pObject->SetWorldTransform(Get_Body_Transform());
	m_WorldBoundBox.Set(m_BoundBox, Translate(m_vPosition));
	m_WorldBoundSphere.Set(m_BoundSphere, Translate(m_vPosition));
}
/******************************************************************************\
*
* Parameters
*
\******************************************************************************/
/*
*/
void CActorBase::Update_Bounds()
{
	float radius = m_pShape->GetRadius();
	float hheight = m_pShape->GetHHeight();
	m_BoundBox.Set(vec3(-radius, -radius, 0.0f), vec3(radius, radius, (radius + hheight) * 2.0f));
	m_BoundSphere.Set(vec3(0.0f, 0.0f, radius + hheight), radius + hheight);

	m_WorldBoundBox.Set(m_BoundBox, Translate(m_vPosition));
	m_WorldBoundSphere.Set(m_BoundSphere, Translate(m_vPosition));
}
/*
*/
void CActorBase::SetIntersectionMask(int mask)
{
	m_pShape->SetIntersectionMask(mask);
}
int CActorBase::GetIntersectionMask() const
{
	return m_pShape->GetIntersectionMask();
}

/*
*/
void CActorBase::SetCollision(int c)
{
	m_nCollision = c;
}
int CActorBase::GetCollision() const
{
	return m_nCollision;
}
void CActorBase::SetCollisionMask(int mask)
{
	m_pShape->SetCollisionMask(mask);
}
int CActorBase::GetCollisionMask() const
{
	return m_pShape->GetCollisionMask();
}

/*
*/
void CActorBase::SetCollisionRadius(float radius)
{
	if (!Compare(m_pShape->GetRadius(), radius))
	{
		m_pDummy->SetPreserveTransform(Mat4(Translate(m_vUp * (radius - m_pShape->GetRadius()))) * m_pDummy->GetTransform());
		m_pShape->SetRadius(radius);
	}
	Update_Bounds();
}
float CActorBase::GetCollisionRadius() const
{
	return m_pShape->GetRadius();
}

void CActorBase::SetCollisionHeight(float height)
{
	if (!Compare(m_pShape->GetHeight(), height))
	{
		m_pDummy->SetPreserveTransform(Mat4(Translate(m_vUp * (height - m_pShape->GetHeight()) * 0.5f)) * m_pDummy->GetTransform());
		m_pShape->SetHeight(height);
	}
	Update_Bounds();
}
float CActorBase::GetCollisionHeight() const
{
	return m_pShape->GetHeight();
}

void CActorBase::SetMinVelocity(float velocity)
{
	m_fMinVelocity = Max(velocity, 0.0f);
}
float CActorBase::GetMinVelocity() const
{
	return m_fMinVelocity;
}
/*

*/
void CActorBase::SetMaxVelocity(float velocity)
{
	m_fMaxVelocity = Max(velocity, 0.0f);
}
float CActorBase::GetMaxVelocity() const
{
	return m_fMaxVelocity;
}

/*
*/
void CActorBase::SetAcceleration(float accel)
{
	m_fAcceleration = Max(accel, 0.0f);
}
float CActorBase::GetAcceleration() const
{
	return m_fAcceleration;
}
<|endoftext|>


<commit_before>#include "Engine.h"
#include "Game.h"
#include "ObjectDummy.h"
#include "BodyRigid.h"
#include "BodyDummy.h"

#include "Physics.h"
#include "ShapeCapsule.h"
#include "ActorBase.h"
#include "Visualizer.h"
#include "sys/SysControl.h"

#ifdef MEMORY_INFO
#define new new(__FILE__, __LINE__)
#endif // MEMORY_INFO

using namespace MathLib;

/*
*/
#define ACTOR_BASE_IFPS             (1.0f / 60.0f)
#define ACTOR_BASE_CLAMP            89.9f
#define ACTOR_BASE_COLLISIONS       4

/*
*/

CActorBase::CActorBase()
{
	m_vUp = vec3(0.0f, 0.0f, 1.0f);
	m_pObject = new CObjectDummy();
	m_pDummy = new CBodyDummy();
	m_pShape = new CShapeCapsule(1.0f, 1.0f);

	m_nFlush = 0;
	m_vPosition = Vec3_zero;
	m_vVelocity = Vec3_zero;
	m_fPhiAngle = 0.0f;			//б		άƽϵУֱYķX֮ļн
	m_fThetaAngle = 0.0f;			//λǣ뵱ǰ˳ʱ߹ĽǶ

	for (int i = 0; i < NUM_STATES; i++)
	{
		m_pStates[i] = 0;
		m_pTimes[i] = 0.0f;
	}

	m_pDummy->SetEnabled(1);
	m_pObject->SetBody(NULL);
	m_pObject->SetBody(m_pDummy);
	m_pShape->SetBody(NULL);
	m_pShape->SetBody(m_pDummy);

	m_pObject->SetWorldTransform(Get_Body_Transform());
	m_pShape->SetRestitution(0.0f);
	m_pShape->SetCollisionMask(2);

	SetEnabled(1);
	SetViewDirection(vec3(0.0f,1.0f,0.0f));
	SetCollision(1);
	SetCollisionRadius(0.3f);
	SetCollisionHeight(1.0f);
	SetFriction(2.0f);
	SetMinVelocity(2.0f);
	SetMaxVelocity(4.0f);
	SetAcceleration(8.0f);
	SetDamping(8.0f);
	SetJumping(1.5f);

	SetGround(0);
	SetCeiling(0);

}
CActorBase::~CActorBase()
{
	m_pDummy->SetObject(NULL);
	delete m_pObject;
	delete m_pDummy;
}

void CActorBase::SetEnabled(int enable)
{
	m_nEnable = enable;
	m_pDummy->SetEnabled(m_nEnable);
}
int CActorBase::IsEnabled() const
{
	return m_nEnable;
}
void CActorBase::Update(float ifps)
{
	if (!m_nEnable)
	{
		return;
	}
	// impulse
	vec3 impulse = vec3_zero;

	// ortho basis
	vec3 tangent, binormal;
	OrthoBasis(m_vUp, tangent, binormal);
	// current basis
	vec3 x = quat(m_vUp, -m_fPhiAngle) * binormal;
	vec3 y = Normalize(Cross(m_vUp, x));
	vec3 z = Normalize(Cross(x, y));
	// handle states
	Update_States(1, ifps);

	// old velocity
	float x_velocity = Dot(x, m_vVelocity);
	float y_velocity = Dot(y, m_vVelocity);
	float z_velocity = Dot(z, m_vVelocity);

	// movement
	if (m_pStates[STATE_FORWARD]) impulse += x;
	if (m_pStates[STATE_BACKWARD]) impulse -= x;
	if (m_pStates[STATE_MOVE_LEFT]) impulse += y;
	if (m_pStates[STATE_MOVE_RIGHT]) impulse -= y;
	impulse.normalize();
	// velocity
	if (m_pStates[STATE_RUN]) impulse *= m_fMaxVelocity;
	else impulse *= m_fMinVelocity;

	// jump
	if (m_pStates[STATE_JUMP] == STATE_BEGIN)
	{
		impulse += z * CMathCore::Sqrt(2.0f * 9.8f * m_fJumping) / (m_fAcceleration * ifps);
	}

	// rotate velocity
	if (GetGround())
	{
		m_vVelocity = x * x_velocity + y * y_velocity + z * z_velocity;
	}
	// time
	float time = ifps * g_Engine.pPhysics->GetScale();
	// target velocity
	float target_velocity = Length(vec2(Dot(x, impulse), Dot(y, impulse)));

	// penetration tolerance
	float penetration = g_Engine.pPhysics->GetPenetrationTolerance();
	float penetration_2 = penetration * 2.0f;

	// frozen linear velocity
	float frozen_velocity = g_Engine.pPhysics->GetFrozenLinearVelocity();

}<commit_msg>Signed-off-by: mrlitong <litongtongxue@gmail.com><commit_after>#include "Engine.h"
#include "Game.h"
#include "ObjectDummy.h"
#include "BodyRigid.h"
#include "BodyDummy.h"

#include "Physics.h"
#include "ShapeCapsule.h"
#include "ActorBase.h"
#include "Visualizer.h"
#include "sys/SysControl.h"

#ifdef MEMORY_INFO
#define new new(__FILE__, __LINE__)
#endif // MEMORY_INFO

using namespace MathLib;

/*
*/
#define ACTOR_BASE_IFPS             (1.0f / 60.0f)
#define ACTOR_BASE_CLAMP            89.9f
#define ACTOR_BASE_COLLISIONS       4

/*
*/

CActorBase::CActorBase()
{
	m_vUp = vec3(0.0f, 0.0f, 1.0f);
	m_pObject = new CObjectDummy();
	m_pDummy = new CBodyDummy();
	m_pShape = new CShapeCapsule(1.0f, 1.0f);

	m_nFlush = 0;
	m_vPosition = Vec3_zero;
	m_vVelocity = Vec3_zero;
	m_fPhiAngle = 0.0f;			//б		άƽϵУֱYķX֮ļн
	m_fThetaAngle = 0.0f;			//λǣ뵱ǰ˳ʱ߹ĽǶ

	for (int i = 0; i < NUM_STATES; i++)
	{
		m_pStates[i] = 0;
		m_pTimes[i] = 0.0f;
	}

	m_pDummy->SetEnabled(1);
	m_pObject->SetBody(NULL);
	m_pObject->SetBody(m_pDummy);
	m_pShape->SetBody(NULL);
	m_pShape->SetBody(m_pDummy);

	m_pObject->SetWorldTransform(Get_Body_Transform());
	m_pShape->SetRestitution(0.0f);
	m_pShape->SetCollisionMask(2);

	SetEnabled(1);
	SetViewDirection(vec3(0.0f,1.0f,0.0f));
	SetCollision(1);
	SetCollisionRadius(0.3f);
	SetCollisionHeight(1.0f);
	SetFriction(2.0f);
	SetMinVelocity(2.0f);
	SetMaxVelocity(4.0f);
	SetAcceleration(8.0f);
	SetDamping(8.0f);
	SetJumping(1.5f);

	SetGround(0);
	SetCeiling(0);

}
CActorBase::~CActorBase()
{
	m_pDummy->SetObject(NULL);
	delete m_pObject;
	delete m_pDummy;
}

void CActorBase::SetEnabled(int enable)
{
	m_nEnable = enable;
	m_pDummy->SetEnabled(m_nEnable);
}
int CActorBase::IsEnabled() const
{
	return m_nEnable;
}
void CActorBase::Update(float ifps)
{
	if (!m_nEnable)
	{
		return;
	}
	// impulse
	vec3 impulse = vec3_zero;

	// ortho basis
	vec3 tangent, binormal;
	OrthoBasis(m_vUp, tangent, binormal);
	// current basis
	vec3 x = quat(m_vUp, -m_fPhiAngle) * binormal;
	vec3 y = Normalize(Cross(m_vUp, x));
	vec3 z = Normalize(Cross(x, y));
	// handle states
	Update_States(1, ifps);

	// old velocity
	float x_velocity = Dot(x, m_vVelocity);
	float y_velocity = Dot(y, m_vVelocity);
	float z_velocity = Dot(z, m_vVelocity);

	// movement
	if (m_pStates[STATE_FORWARD]) impulse += x;
	if (m_pStates[STATE_BACKWARD]) impulse -= x;
	if (m_pStates[STATE_MOVE_LEFT]) impulse += y;
	if (m_pStates[STATE_MOVE_RIGHT]) impulse -= y;
	impulse.normalize();
	// velocity
	if (m_pStates[STATE_RUN]) impulse *= m_fMaxVelocity;
	else impulse *= m_fMinVelocity;

	// jump
	if (m_pStates[STATE_JUMP] == STATE_BEGIN)
	{
		impulse += z * CMathCore::Sqrt(2.0f * 9.8f * m_fJumping) / (m_fAcceleration * ifps);
	}

	// rotate velocity
	if (GetGround())
	{
		m_vVelocity = x * x_velocity + y * y_velocity + z * z_velocity;
	}
	// time
	float time = ifps * g_Engine.pPhysics->GetScale();
	// target velocity
	float target_velocity = Length(vec2(Dot(x, impulse), Dot(y, impulse)));

	// penetration tolerance
	float penetration = g_Engine.pPhysics->GetPenetrationTolerance();
	float penetration_2 = penetration * 2.0f;

	// frozen linear velocity
	float frozen_velocity = g_Engine.pPhysics->GetFrozenLinearVelocity();

	// friction
	float friction = 0.0f;
	if (target_velocity < EPSILON)
	{
		friction = m_fFriction;
	}

}<|endoftext|>